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



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018