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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at dom jul 14 20:00:11 CEST 2019