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 
157  auto AtDO = std::chrono::milliseconds(AtUI);
158  outObservation.timestamp = m_timeStartTT + AtDO;
159  }
160 
161  // And the scan ranges:
162  outObservation.rightToLeft = true;
163 
164  outObservation.aperture =
165  nRanges * 2 * M_PI / m_sensor_info.scans_per_360deg;
166 
167  outObservation.maxRange = m_sensor_info.d_max;
168  outObservation.stdError = 0.010f;
169  outObservation.sensorPose = m_sensorPose;
170  outObservation.sensorLabel = m_sensorLabel;
171 
172  outObservation.resizeScan(nRanges);
173  char* ptr = (char*)&m_rcv_data[4];
174 
175  if (m_intensity) outObservation.setScanHasIntensity(true);
176 
177  for (int i = 0; i < nRanges; i++)
178  {
179  int b1 = (*ptr++) - 0x30;
180  int b2 = (*ptr++) - 0x30;
181  int b3 = (*ptr++) - 0x30;
182 
183  int range_mm = ((b1 << 12) | (b2 << 6) | b3);
184 
185  outObservation.setScanRange(i, range_mm * 0.001f);
186  outObservation.setScanRangeValidity(
187  i, range_mm >= 20 &&
188  (outObservation.scan[i] <= outObservation.maxRange));
189 
190  if (m_intensity)
191  {
192  int b4 = (*ptr++) - 0x30;
193  int b5 = (*ptr++) - 0x30;
194  int b6 = (*ptr++) - 0x30;
195  outObservation.setScanIntensity(i, ((b4 << 12) | (b5 << 6) | b6));
196  }
197  }
198 
199  // Do filter:
202  // Do show preview:
204 
205  outThereIsObservation = true;
206 }
207 
208 /*-------------------------------------------------------------
209  loadConfig_sensorSpecific
210 -------------------------------------------------------------*/
212  const mrpt::config::CConfigFileBase& configSource,
213  const std::string& iniSection)
214 {
215  m_reduced_fov =
216  DEG2RAD(configSource.read_float(iniSection, "reduced_fov", 0)),
217 
219  configSource.read_int(iniSection, "HOKUYO_motorSpeed_rpm", 0);
221  configSource.read_float(iniSection, "pose_x", 0),
222  configSource.read_float(iniSection, "pose_y", 0),
223  configSource.read_float(iniSection, "pose_z", 0),
224  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
225  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
226  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
227 
229  configSource.read_bool(iniSection, "HOKUYO_HS_mode", m_highSensMode);
230 
231 #ifdef _WIN32
232  m_com_port =
233  configSource.read_string(iniSection, "COM_port_WIN", m_com_port);
234 #else
235  m_com_port =
236  configSource.read_string(iniSection, "COM_port_LIN", m_com_port);
237 #endif
238 
239  m_ip_dir = configSource.read_string(iniSection, "IP_DIR", m_ip_dir);
240  m_port_dir = configSource.read_int(iniSection, "PORT_DIR", m_port_dir);
241 
242  ASSERTMSG_(
243  !m_com_port.empty() || !m_ip_dir.empty(),
244  "Either COM_port or IP_DIR must be defined in the configuration file!");
245  ASSERTMSG_(
246  m_com_port.empty() || m_ip_dir.empty(),
247  "Both COM_port and IP_DIR set! Please, define only one of them.");
248  if (!m_ip_dir.empty())
249  {
250  ASSERTMSG_(
251  m_port_dir,
252  "A TCP/IP port number `PORT_DIR` must be specified for Ethernet "
253  "connection");
254  }
255 
257  iniSection, "disable_firmware_timestamp", m_disable_firmware_timestamp);
258  m_intensity = configSource.read_bool(iniSection, "intensity", m_intensity),
259 
261  scan_interval, int, m_scan_interval, configSource, iniSection);
262 
263  // Parent options:
264  C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
265 }
266 
267 /*-------------------------------------------------------------
268  turnOn
269 -------------------------------------------------------------*/
271 {
272  MRPT_START
273 
274  // Bound?
275  if (!ensureStreamIsOpen()) return false;
276 
277  // If we are over a serial link, set it up:
278  if (m_ip_dir.empty())
279  {
280  CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
281 
282  if (COM != nullptr)
283  {
284  // It is a COM:
285  COM->setConfig(19200);
286  COM->setTimeouts(100, 0, 200, 0, 0);
287 
288  // Assure the laser is off and quiet:
289  switchLaserOff();
290  std::this_thread::sleep_for(10ms);
291 
292  COM->purgeBuffers();
293  std::this_thread::sleep_for(10ms);
294 
295  COM->setConfig(115200);
296  switchLaserOff();
297  std::this_thread::sleep_for(10ms);
298  COM->purgeBuffers();
299  std::this_thread::sleep_for(10ms);
300  COM->setConfig(19200);
301  }
302 
303  if (COM != nullptr)
304  {
305  // Set 115200 baud rate:
306  setHighBaudrate();
307  enableSCIP20();
308  COM->setConfig(115200);
309  }
310  }
311  else
312  {
313  CClientTCPSocket* COM = dynamic_cast<CClientTCPSocket*>(m_stream);
314 
315  if (COM != nullptr)
316  {
317  // Assure the laser is off and quiet:
318  switchLaserOff();
319  std::this_thread::sleep_for(10ms);
320 
321  purgeBuffers();
322  std::this_thread::sleep_for(10ms);
323 
324  switchLaserOff();
325  std::this_thread::sleep_for(10ms);
326  purgeBuffers();
327  }
328  }
329 
330  if (!enableSCIP20()) return false;
331 
332  // Turn on the laser:
333  if (!switchLaserOn()) return false;
334 
335  // Set the motor speed:
336  if (m_motorSpeed_rpm)
337  if (!setMotorSpeed(m_motorSpeed_rpm)) return false;
338 
339  // Set HS mode:
341 
342  // Display sensor information:
343  if (!displaySensorInfo(&m_sensor_info)) return false;
344 
345  // Set for scanning angles:
348 
349  // Artificially reduced FOV?
350  if (m_reduced_fov > 0 && m_reduced_fov < 2 * M_PI)
351  {
352  int center = (m_lastRange + m_firstRange) >> 1;
353  const int half_range = static_cast<int>(
356  1;
357  m_firstRange = center - half_range;
358  m_lastRange = center + half_range;
360  "[HOKUYO::turnOn] Using reduced FOV: ranges ["
361  << m_firstRange << "-" << m_lastRange << "] for "
362  << RAD2DEG(m_reduced_fov) << " deg. FOV");
363  }
364 
365  if (!displayVersionInfo())
366  {
367  // return false; // It's not SO important
368  }
369 
370  // Start!
371  if (!startScanningMode()) return false;
372 
373  return true;
374 
375  MRPT_END
376 }
377 
378 /*-------------------------------------------------------------
379  turnOff
380 -------------------------------------------------------------*/
382 {
383  // Turn off the laser:
384  if (!switchLaserOff()) return false;
385 
386  return true;
387 }
388 
390 {
391  char rcv_status0, rcv_status1;
392  if (!ensureStreamIsOpen()) return false;
393 
395  "[CHokuyoURG::setHighBaudrate] Changing baudrate to 115200...");
396 
397  // Send command:
398  sendCmd("SS115200\x0A");
399 
400  // Receive response:
401  if (!receiveResponse(rcv_status0, rcv_status1))
402  {
404  "[CHokuyoURG::setHighBaudrate] Error waiting for response");
405  return false;
406  }
407 
408  MRPT_LOG_DEBUG("OK\n");
409  return true;
410 }
411 
412 /*-------------------------------------------------------------
413  assureBufferHasBytes
414 -------------------------------------------------------------*/
415 bool CHokuyoURG::assureBufferHasBytes(const size_t nDesiredBytes)
416 {
417  ASSERT_(nDesiredBytes < m_rx_buffer.capacity());
418 
419  if (m_rx_buffer.size() >= nDesiredBytes)
420  {
421  return true;
422  }
423  else
424  {
425  // Try to read more bytes:
426  uint8_t buf[128];
427  const size_t to_read = std::min(m_rx_buffer.available(), sizeof(buf));
428 
429  try
430  {
431  size_t nRead;
432 
433  if (!m_ip_dir.empty())
434  {
435  CClientTCPSocket* client =
436  dynamic_cast<CClientTCPSocket*>(m_stream);
437  nRead = client->readAsync(buf, to_read, 100, 10);
438  }
439  else
440  {
441  nRead = m_stream->Read(buf, to_read);
442  }
443 
444  m_rx_buffer.push_many(buf, nRead);
445  }
446  catch (std::exception&)
447  {
448  // 0 bytes read
449  }
450 
451  return (m_rx_buffer.size() >= nDesiredBytes);
452  }
453 }
454 
455 bool CHokuyoURG::receiveResponse(char& rcv_status0, char& rcv_status1)
456 {
457  m_rcv_data.clear();
458  if (!ensureStreamIsOpen()) return false;
459  ASSERT_(!m_lastSentMeasCmd.empty());
460 
461  try
462  {
463  // Process response:
464  // ---------------------------------
465 
466  // COMMAND ECHO ---------
467  unsigned int i = 0;
468  const unsigned int verifLen = m_lastSentMeasCmd.size();
469 
470  if (verifLen)
471  {
472  do
473  {
474  if (!assureBufferHasBytes(verifLen - i)) return false;
475 
476  // If matches the echo, go on:
477  if (m_rx_buffer.pop() == m_lastSentMeasCmd[i])
478  i++;
479  else
480  i = 0;
481  } while (i < verifLen);
482  }
483 
484  // Now, the status bytes:
485  if (!assureBufferHasBytes(2)) return false;
486 
487  rcv_status0 = m_rx_buffer.pop();
488  rcv_status1 = m_rx_buffer.pop();
489 
490  // In SCIP2.0, there is an additional sum char:
491  if (rcv_status1 != 0x0A)
492  {
493  // Yes, it is SCIP2.0
494  if (!assureBufferHasBytes(1)) return false;
495 
496  // Ignore this byte: sumStatus
497  m_rx_buffer.pop();
498  }
499  else
500  {
501  // Continue, it seems a SCIP1.1 response...
502  }
503 
504  // After the status bytes, there is a LF:
505  if (!assureBufferHasBytes(1)) return false;
506  char nextChar = m_rx_buffer.pop();
507  if (nextChar != 0x0A) return false;
508 
509  // -----------------------------------------------------------------------------
510  // Now the data:
511  // There's a problem here, we don't know in advance how many bytes to
512  // read,
513  // so rely on the serial port class implemented buffer and call many
514  // times
515  // the read method with only 1 byte each time:
516  // -----------------------------------------------------------------------------
517  bool lastWasLF = false;
518  i = 0;
519  for (;;)
520  {
521  if (!assureBufferHasBytes(1))
522  {
523  return false;
524  }
525  m_rcv_data.push_back(m_rx_buffer.pop());
526  i++; // One more byte in the buffer
527 
528  // No data?
529  if (i == 1 && m_rcv_data[0] == 0x0A)
530  {
531  m_rcv_data.clear();
532  return true;
533  }
534 
535  // Is it a LF?
536  if (m_rcv_data[i - 1] == 0x0A)
537  {
538  if (!lastWasLF)
539  {
540  // Discard SUM+LF
541  ASSERT_(i >= 2);
542  i -= 2;
543  m_rcv_data.resize(i);
544  }
545  else
546  {
547  // Discard this last LF:
548  i--;
549 
550  // Done!
551  m_rcv_data.resize(i);
553  "[Hokuyo] receiveResponse(): RX `" << m_rcv_data
554  << "`");
555 
556  if (rcv_status0 != '0' &&
557  (rcv_status0 != '9' && rcv_status1 != '9'))
558  {
560  "[Hokuyo] Error LIDAR status: "
561  << (int)rcv_status0 << " after command: `"
562  << m_lastSentMeasCmd << "`");
563  return false;
564  }
565 
566  return true;
567  }
568  lastWasLF = true;
569  }
570  else
571  lastWasLF = false;
572  }
573  }
574  catch (std::exception& e)
575  {
577  "[Hokuyo] receiveResponse() Exception: %s", e.what());
578  return false;
579  }
580  catch (...)
581  {
582  return false; // Serial port timeout,...
583  }
584 }
585 
587 {
588  char rcv_status0, rcv_status1;
589  if (!ensureStreamIsOpen()) return false;
590 
592  "[CHokuyoURG::enableSCIP20] Changing protocol to SCIP2.0...");
593 
594  // Send command:
595  sendCmd("SCIP2.0\x0A");
596 
597  // Receive response:
598  if (!receiveResponse(rcv_status0, rcv_status1))
599  {
601  __CURRENT_FUNCTION_NAME__ << ": Error in response");
602  return false;
603  }
604 
605  MRPT_LOG_DEBUG("OK\n");
606  return true;
607 }
608 
610 {
611  char rcv_status0, rcv_status1;
612 
613  if (!ensureStreamIsOpen()) return false;
614 
615  MRPT_LOG_DEBUG("[CHokuyoURG::switchLaserOn] Switching laser ON...");
616 
617  // Send command:
618  sendCmd("BM\x0A");
619 
620  // Receive response:
621  if (!receiveResponse(rcv_status0, rcv_status1))
622  {
624  __CURRENT_FUNCTION_NAME__ << ": Error in response");
625  return false;
626  }
627 
628  MRPT_LOG_DEBUG("OK\n");
629  return true;
630 }
631 
633 {
634  char rcv_status0, rcv_status1;
635 
636  if (!ensureStreamIsOpen()) return false;
637 
638  MRPT_LOG_DEBUG("[CHokuyoURG::switchLaserOff] Switching laser OFF...");
639 
640  // Send command:
641  sendCmd("QT\x0A");
642 
643  // Receive response:
644  if (!receiveResponse(rcv_status0, rcv_status1))
645  {
647  __CURRENT_FUNCTION_NAME__ << ": Error in response");
648  return false;
649  }
650 
651  MRPT_LOG_DEBUG("OK\n");
652  return true;
653 }
654 
655 void CHokuyoURG::setScanInterval(unsigned int skipScanCount)
656 {
657  m_scan_interval = skipScanCount;
658 }
659 unsigned int CHokuyoURG::getScanInterval() const { return m_scan_interval; }
660 bool CHokuyoURG::setMotorSpeed(int motoSpeed_rpm)
661 {
662  char rcv_status0, rcv_status1;
663  if (!ensureStreamIsOpen()) return false;
664 
666  "[CHokuyoURG::setMotorSpeed] Setting to %i rpm...", motoSpeed_rpm);
667 
668  // Send command:
669  int motorSpeedCode = (600 - motoSpeed_rpm) / 6;
670  if (motorSpeedCode < 0 || motorSpeedCode > 10)
671  {
674  << " Motorspeed must be in the range 540-600 rpm");
675  return false;
676  }
677 
678  char cmd[20];
679  os::sprintf(cmd, 20, "CR%02i\x0A", motorSpeedCode);
680  sendCmd(cmd);
681 
682  // Receive response:
683  if (!receiveResponse(rcv_status0, rcv_status1))
684  {
686  __CURRENT_FUNCTION_NAME__ << ": Error in response");
687  return false;
688  }
689 
690  MRPT_LOG_DEBUG("OK\n");
691  return true;
692 }
693 
694 /*-------------------------------------------------------------
695  setHighSensitivityMode
696 -------------------------------------------------------------*/
698 {
699  char rcv_status0, rcv_status1;
700  if (!ensureStreamIsOpen()) return false;
701 
703  "[CHokuyoURG::setHighSensitivityMode] Setting HS mode to: %s...",
704  enabled ? "true" : "false");
705 
706  // Send command:
707  char cmd[20];
708  os::sprintf(cmd, 20, "HS%i\x0A", enabled ? 1 : 0);
709  sendCmd(cmd);
710 
711  // Receive response:
712  if (!receiveResponse(rcv_status0, rcv_status1))
713  {
715  __CURRENT_FUNCTION_NAME__ << ": Error in response");
716  return false;
717  }
718 
719  MRPT_LOG_DEBUG("OK\n");
720  return true;
721 }
722 
723 /*-------------------------------------------------------------
724  setIntensityMode
725 -------------------------------------------------------------*/
727 {
728  m_intensity = enabled;
729  return true;
730 }
731 
733 {
734  char rcv_status0, rcv_status1;
735  if (!ensureStreamIsOpen()) return false;
736 
737  MRPT_LOG_DEBUG("[CHokuyoURG::displayVersionInfo] Asking info...");
738 
739  // Send command:
740  sendCmd("VV\x0A");
741 
742  // Receive response:
743  if (!receiveResponse(rcv_status0, rcv_status1))
744  {
746  __CURRENT_FUNCTION_NAME__ << ": Error in response");
747  return false;
748  }
749 
750  MRPT_LOG_DEBUG("OK\n");
751 
752  // PRINT:
753  for (auto& c : m_rcv_data)
754  if (c == ';') c = '\n';
755  m_rcv_data[m_rcv_data.size()] = '\0';
756 
757  if (!m_rcv_data.empty())
758  {
760  "\n------------- HOKUYO Scanner: Version Information ------\n"
761  << &m_rcv_data[0]
762  << "\n"
763  "-------------------------------------------------------\n\n");
764  }
765  return true;
766 }
767 
768 /*-------------------------------------------------------------
769  displaySensorInfo
770 -------------------------------------------------------------*/
772 {
773  char rcv_status0, rcv_status1;
774  if (!ensureStreamIsOpen()) return false;
775 
776  MRPT_LOG_DEBUG("[CHokuyoURG::displaySensorInfo] Asking for info...");
777 
778  // Send command:
779  sendCmd("PP\x0A");
780 
781  // Receive response:
782  if (!receiveResponse(rcv_status0, rcv_status1))
783  {
785  __CURRENT_FUNCTION_NAME__ << ": Error in response");
786  return false;
787  }
788  MRPT_LOG_DEBUG("OK\n");
789 
790  // PRINT:
791  for (auto& c : m_rcv_data)
792  if (c == ';') c = '\n';
793  m_rcv_data[m_rcv_data.size()] = '\0';
794 
795  if (!m_rcv_data.empty())
796  {
798  "\n------------- HOKUYO Scanner: Product Information ------\n"
799  << &m_rcv_data[0]
800  << "\n"
801  "-------------------------------------------------------\n\n");
802  }
803 
804  // Parse the data:
805  if (out_data)
806  {
807  const char* ptr;
808 
809  if (nullptr != (ptr = strstr(&m_rcv_data[0], "DMAX:")))
810  out_data->d_max = 0.001 * atoi(ptr + 5);
811  else
812  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
813 
814  if (nullptr != (ptr = strstr(&m_rcv_data[0], "DMIN:")))
815  out_data->d_min = 0.001 * atoi(ptr + 5);
816  else
817  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
818 
819  if (nullptr != (ptr = strstr(&m_rcv_data[0], "ARES:")))
820  out_data->scans_per_360deg = atoi(ptr + 5);
821  else
822  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
823 
824  if (nullptr != (ptr = strstr(&m_rcv_data[0], "SCAN:")))
825  out_data->motor_speed_rpm = atoi(ptr + 5);
826  else
827  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
828 
829  if (nullptr != (ptr = strstr(&m_rcv_data[0], "AMIN:")))
830  out_data->scan_first = atoi(ptr + 5);
831  else
832  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
833 
834  if (nullptr != (ptr = strstr(&m_rcv_data[0], "AMAX:")))
835  out_data->scan_last = atoi(ptr + 5);
836  else
837  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
838 
839  if (nullptr != (ptr = strstr(&m_rcv_data[0], "AFRT:")))
840  out_data->scan_front = atoi(ptr + 5);
841  else
842  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
843 
844  if (nullptr != (ptr = strstr(&m_rcv_data[0], "MODL:")))
845  {
846  char aux[30];
847  memcpy(aux, ptr + 5, 8);
848  aux[8] = '\0';
849  out_data->model = aux;
850  }
851  else
852  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
853  }
854 
855  return true;
856 }
857 
859 {
860  char rcv_status0, rcv_status1;
861  if (!ensureStreamIsOpen()) return false;
862 
863  MRPT_LOG_DEBUG("[CHokuyoURG::startScanningMode] Starting scanning mode...");
864 
865  // Send command:
866  // 'M' 'D'
867  // 'XXXX' (starting step)
868  // 'XXXX' (end step)
869  // 'XX' (cluster count)
870  // 'X' (scan interval)
871  // 'XX' (number of scans)
872  char cmd[50];
873  unsigned int scan_interval = m_scan_interval;
874  if (scan_interval > 9) scan_interval = 9;
875  os::sprintf(
876  cmd, 50, "M%c%04u%04u01%u00\x0A", m_intensity ? 'E' : 'D', m_firstRange,
877  m_lastRange, scan_interval);
878 
879  sendCmd(cmd);
880 
881  // Receive response:
882  if (!receiveResponse(rcv_status0, rcv_status1))
883  {
885  __CURRENT_FUNCTION_NAME__ << ": Error in response");
886  return false;
887  }
888 
889  MRPT_LOG_DEBUG("OK\n");
890  return true;
891 }
892 
894 {
895  MRPT_START
896 
897  if (m_stream)
898  {
899  // Socket or USB connection?
900  if (!m_ip_dir.empty() && m_port_dir)
901  {
902  // Has the port been disconected (USB serial ports)??
903  CClientTCPSocket* COM = dynamic_cast<CClientTCPSocket*>(m_stream);
904 
905  if (COM != nullptr)
906  {
907  if (COM->isConnected()) return true;
908 
909  // It has been disconnected... try to reconnect:
911  "[CHokuyoURG] Socket connection lost! trying to "
912  "reconnect...");
913 
914  try
915  {
916  COM->connect(m_ip_dir, m_port_dir);
917  // OK, reconfigure the laser:
918  turnOn();
919  return true;
920  }
921  catch (...)
922  {
923  // Not yet..
924  return false;
925  }
926  }
927  else
928  {
929  return true; // Assume OK
930  }
931  }
932  else
933  {
934  // Has the port been disconected (USB serial ports)??
935  CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
936  if (COM != nullptr)
937  {
938  if (COM->isOpen()) return true;
939 
940  // It has been disconnected... try to reconnect:
943  << ": Serial port connection lost! Trying to reconnect...");
944 
945  try
946  {
947  COM->open();
948  // OK, reconfigure the laser:
949  turnOn();
950  return true;
951  }
952  catch (...)
953  {
954  // Not yet..
955  return false;
956  }
957  }
958  else
959  {
960  return true; // Assume OK
961  }
962  }
963  }
964  else
965  {
966  if (m_com_port.empty() && m_ip_dir.empty() && !m_port_dir)
967  {
969  "No stream bound to the laser nor COM serial port or ip and "
970  "port provided in 'm_com_port','m_ip_dir' and 'm_port_dir'");
971  }
972 
973  if (!m_ip_dir.empty())
974  {
975  // Try to open the serial port:
976  CClientTCPSocket* theCOM = new CClientTCPSocket();
977 
979  __CURRENT_FUNCTION_NAME__ << " Connecting to " << m_ip_dir
980  << ":" << m_port_dir);
981  theCOM->connect(m_ip_dir, m_port_dir);
982 
983  if (!theCOM->isConnected())
984  {
987  << " Cannot connect with the server '" << m_com_port
988  << "'");
989  delete theCOM;
990  return false;
991  }
992 
993  // Bind:
994  bindIO(theCOM);
995 
997  }
998  else
999  {
1000  // Try to open the serial port:
1001  CSerialPort* 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 
1018  return true;
1019  }
1020  MRPT_END
1021 }
1022 
1024 {
1026 
1027  if (!ensureStreamIsOpen()) return;
1028 
1029  if (!turnOn())
1030  {
1031  MRPT_LOG_ERROR("[Hokuyo] Error initializing HOKUYO scanner");
1032  return;
1033  }
1034 }
1035 
1037 {
1038  if (!ensureStreamIsOpen()) return;
1039 
1040  if (m_ip_dir.empty())
1041  {
1042  CSerialPort* COM = dynamic_cast<CSerialPort*>(m_stream);
1043  if (COM != nullptr)
1044  {
1045  COM->purgeBuffers();
1046  }
1047  }
1048  else // Socket connection
1049  {
1050  CClientTCPSocket* COM = dynamic_cast<CClientTCPSocket*>(m_stream);
1051 
1052  size_t to_read = COM->getReadPendingBytes();
1053 
1054  if (to_read)
1055  {
1056  void* buf = malloc(sizeof(uint8_t) * to_read);
1057 
1058  size_t nRead = m_stream->Read(buf, to_read);
1059 
1060  if (nRead != to_read)
1062  "Error in purge buffers: read and expected number of bytes "
1063  "are different.");
1064 
1065  free(buf);
1066  }
1067  }
1068 }
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:655
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:263
uint32_t m_timeStartUI
Time of the first data packet, for synchronization purposes.
Definition: CHokuyoURG.h:278
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:211
int m_firstRange
The first and last ranges to consider from the scan.
Definition: CHokuyoURG.h:96
#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:250
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
void connect(const std::string &remotePartAddress, unsigned short remotePartTCPPort, unsigned int timeout_ms=0)
Establishes a connection with a remote part.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
bool m_intensity
Get intensity from lidar scan (default: false)
Definition: CHokuyoURG.h:285
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:659
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:102
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:415
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:87
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:893
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:609
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:270
std::string m_lastSentMeasCmd
The last sent measurement command (MDXXX), including the last 0x0A.
Definition: CHokuyoURG.h:105
mrpt::gui::CDisplayWindow3D::Ptr m_win
Definition: CHokuyoURG.h:109
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:771
const GLubyte * c
Definition: glext.h:6313
Used in CHokuyoURG::displayVersionInfo.
Definition: CHokuyoURG.h:80
void setScanIntensity(const size_t i, const int val)
bool setIntensityMode(bool enabled)
If true scans will capture intensity.
Definition: CHokuyoURG.cpp:726
bool displayVersionInfo()
Ask to the device, and print to the debug stream, details about the firmware version,serial number,...
Definition: CHokuyoURG.cpp:732
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:455
#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:281
bool enableSCIP20()
Enables the SCIP2.0 protocol (this must be called at the very begining!).
Definition: CHokuyoURG.cpp:586
void purgeBuffers()
Purge tx and rx buffers.
mrpt::system::TTimeStamp m_timeStartTT
Definition: CHokuyoURG.h:282
#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:632
int m_motorSpeed_rpm
The motor speed (default=600rpm)
Definition: CHokuyoURG.h:98
#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:89
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:858
#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:270
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:660
double d_min
Min/Max ranges, in meters.
Definition: CHokuyoURG.h:85
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:75
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:389
poses::CPose3D m_sensorPose
The sensor 6D pose:
Definition: CHokuyoURG.h:100
unsigned int m_scan_interval
Definition: CHokuyoURG.h:286
#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:273
bool m_highSensMode
High sensitivity [HS] mode (default: false)
Definition: CHokuyoURG.h:108
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:83
Serial and networking devices and utilities.
double m_reduced_fov
Used to reduce artificially the interval of scan ranges.
Definition: CHokuyoURG.h:258
int motor_speed_rpm
Standard motor speed, rpm.
Definition: CHokuyoURG.h:91
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:381
bool setHighSensitivityMode(bool enabled)
Changes the high sensitivity mode (HS) (default: false)
Definition: CHokuyoURG.cpp:697
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:267
void setScanRangeValidity(const size_t i, const bool val)
int scans_per_360deg
Number of measuremens per 360 degrees.
Definition: CHokuyoURG.h:87



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020