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 }
size_t available() const
The maximum number of elements that can be written ("push") without rising an overflow error...
void setScanInterval(unsigned int skipScanCount)
Set the skip scan count (0 means send all scans).
Definition: CHokuyoURG.cpp:656
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
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
Definition: CHokuyoURG.h:280
virtual ~CHokuyoURG()
Destructor: turns the laser off.
Definition: CHokuyoURG.cpp:51
#define MRPT_START
Definition: exceptions.h:262
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
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
int m_firstRange
The first and last ranges to consider from the scan.
Definition: CHokuyoURG.h:98
#define min(a, b)
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
#define MRPT_LOG_ERROR_FMT(_FMT_STRING,...)
std::string m_rcv_data
temp buffer for incoming data packets
Definition: CHokuyoURG.h:252
double RAD2DEG(const double x)
Radians to degrees.
size_t capacity() const
Return the maximum capacity of the buffer.
A communications serial port built as an implementation of a utils::CStream.
Definition: CSerialPort.h:43
void connect(const std::string &remotePartAddress, unsigned short remotePartTCPPort, unsigned int timeout_ms=0)
Establishes a connection with a remote part.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
bool m_intensity
Get intensity from lidar scan (default: false)
Definition: CHokuyoURG.h:287
bool isConnected()
Returns true if this objects represents a successfully connected socket.
void setScanRange(const size_t i, const float val)
std::string m_sensorLabel
See CGenericSensor.
double DEG2RAD(const double x)
Degrees to radians.
unsigned int getScanInterval() const
Definition: CHokuyoURG.cpp:660
virtual size_t Read(void *Buffer, size_t Count)=0
Introduces a pure virtual method responsible for reading from the stream.
mrpt::containers::circular_buffer< uint8_t > m_rx_buffer
Auxiliary buffer for readings.
Definition: CHokuyoURG.h:104
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
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account...
void open()
Open the port.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
Contains classes for various device interfaces.
void setScanHasIntensity(bool setHasIntensityFlag)
Marks this scan as having or not intensity data.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
STL namespace.
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
bool ensureStreamIsOpen()
Returns true if there is a valid stream bound to the laser scanner, otherwise it first try to open th...
Definition: CHokuyoURG.cpp:894
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
void setConfig(int baudRate, int parity=0, int bits=8, int nStopBits=1, bool enableFlowControl=false)
Changes the configuration of the port.
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
bool switchLaserOn()
Switchs the laser on.
Definition: CHokuyoURG.cpp:610
mrpt::io::CStream * m_stream
The I/O channel (will be nullptr if not bound).
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
unsigned int m_port_dir
If set to non-empty and m_ip_dir too, the program will try to connect to a Hokuyo using Ethernet comm...
Definition: CHokuyoURG.h:272
std::string m_lastSentMeasCmd
The last sent measurement command (MDXXX), including the last 0x0A.
Definition: CHokuyoURG.h:107
mrpt::gui::CDisplayWindow3D::Ptr m_win
Definition: CHokuyoURG.h:111
unsigned char uint8_t
Definition: rptypes.h:41
float maxRange
The maximum range allowed by the device, in meters (e.g.
const int MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE
Definition: CHokuyoURG.cpp:28
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This class allows loading and storing values and vectors of different types from a configuration text...
T pop()
Retrieve an element from the buffer.
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< float > > scan
The range values of the scan, in meters.
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
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
const GLubyte * c
Definition: glext.h:6313
Used in CHokuyoURG::displayVersionInfo.
Definition: CHokuyoURG.h:82
void setScanIntensity(const size_t i, const int val)
bool setIntensityMode(bool enabled)
If true scans will capture intensity.
Definition: CHokuyoURG.cpp:727
bool displayVersionInfo()
Ask to the device, and print to the debug stream, details about the firmware version,serial number,...
Definition: CHokuyoURG.cpp:733
bool isOpen() const
Returns if port has been correctly open.
This namespace contains representation of robot actions and observations.
bool receiveResponse(char &rcv_status0, char &rcv_status1)
Waits for a response from the device.
Definition: CHokuyoURG.cpp:456
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
int m_timeStartSynchDelay
Counter to discard to first few packets before setting the correspondence between device and computer...
Definition: CHokuyoURG.h:283
bool enableSCIP20()
Enables the SCIP2.0 protocol (this must be called at the very begining!).
Definition: CHokuyoURG.cpp:587
void purgeBuffers()
Purge tx and rx buffers.
mrpt::system::TTimeStamp m_timeStartTT
Definition: CHokuyoURG.h:284
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
GLsizei const GLchar ** string
Definition: glext.h:4101
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
size_t size() const
Return the number of elements available for read ("pop") in the buffer (this is NOT the maximum size ...
void setTimeouts(int ReadIntervalTimeout, int ReadTotalTimeoutMultiplier, int ReadTotalTimeoutConstant, int WriteTotalTimeoutMultiplier, int WriteTotalTimeoutConstant)
Changes the timeouts of the port, in milliseconds.
bool switchLaserOff()
Switchs the laser off.
Definition: CHokuyoURG.cpp:633
int m_motorSpeed_rpm
The motor speed (default=600rpm)
Definition: CHokuyoURG.h:100
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
void purgeBuffers()
Empties the RX buffers of the serial port.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
int scan_first
First, last, and front step of the scanner angular span.
Definition: CHokuyoURG.h:91
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
bool startScanningMode()
Start the continuous scanning mode, using parameters stored in the object (loaded from the ...
Definition: CHokuyoURG.cpp:859
#define MRPT_LOG_ERROR(_STRING)
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
virtual size_t Write(const void *Buffer, size_t Count)=0
Introduces a pure virtual method responsible for writing to the stream.
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
#define MRPT_END
Definition: exceptions.h:266
void loadCommonParams(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
Should be call by derived classes at "loadConfig" (loads exclusion areas AND exclusion angles)...
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:239
A TCP socket that can be connected to a TCP server, implementing MRPT&#39;s CStream interface for passing...
bool turnOn()
Enables the scanning mode (which may depend on the specific laser device); this must be called before...
Definition: CHokuyoURG.cpp:271
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
bool setMotorSpeed(int motoSpeed_rpm)
Changes the motor speed in rpm&#39;s (default 600rpm)
Definition: CHokuyoURG.cpp:661
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
double d_min
Min/Max ranges, in meters.
Definition: CHokuyoURG.h:87
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
This software driver implements the protocol SCIP-2.0 for interfacing HOKUYO URG/UTM/UXM/UST laser sc...
Definition: CHokuyoURG.h:77
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
void initialize()
Turns the laser on.
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
bool setHighBaudrate()
Passes to 115200bps bitrate.
Definition: CHokuyoURG.cpp:390
poses::CPose3D m_sensorPose
The sensor 6D pose:
Definition: CHokuyoURG.h:102
unsigned int m_scan_interval
Definition: CHokuyoURG.h:288
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
TSensorInfo m_sensor_info
The information gathered when the laser is first open.
Definition: CHokuyoURG.h:275
mrpt::system::TTimeStamp secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:224
bool m_highSensMode
High sensitivity [HS] mode (default: false)
Definition: CHokuyoURG.h:110
void push_many(T *array_elements, size_t count)
Insert an array of elements in the buffer.
size_t getReadPendingBytes()
Return the number of bytes already in the receive queue (they can be read without waiting) ...
std::string model
The sensor model.
Definition: CHokuyoURG.h:85
Serial and networking devices and utilities.
double m_reduced_fov
Used to reduce artificially the interval of scan ranges.
Definition: CHokuyoURG.h:260
int motor_speed_rpm
Standard motor speed, rpm.
Definition: CHokuyoURG.h:93
size_t readAsync(void *Buffer, const size_t Count, const int timeoutStart_ms=-1, const int timeoutBetween_ms=-1)
A method for reading from the socket with an optional timeout.
#define __CURRENT_FUNCTION_NAME__
A macro for obtaining the name of the current function:
Definition: common.h:150
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
unsigned __int32 uint32_t
Definition: rptypes.h:47
bool turnOff()
Disables the scanning mode (this can be used to turn the device in low energy mode, if available)
Definition: CHokuyoURG.cpp:382
bool setHighSensitivityMode(bool enabled)
Changes the high sensitivity mode (HS) (default: false)
Definition: CHokuyoURG.cpp:698
void bindIO(mrpt::io::CStream *streamIO)
Binds the object to a given I/O channel.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
void sendCmd(const char *str)
Definition: CHokuyoURG.cpp:63
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
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:356
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
void setScanRangeValidity(const size_t i, const bool val)
int scans_per_360deg
Number of measuremens per 360 degrees.
Definition: CHokuyoURG.h:89



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019