Main MRPT website > C++ reference for MRPT 1.5.7
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 
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 
32 CHokuyoURG::CHokuyoURG()
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 
43  if (m_stream) {
44  turnOff();
45 
47  delete m_stream;
48  m_stream = nullptr;
49  }
50  m_win.reset(NULL);
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 
136  /* Board time is ms */
137  const double At_secs = AtUI*1e-3;
138  const double At_secs_CPU = mrpt::system::timeDifference(m_timeStartTT, mrpt::system::now());
139  const double clock_drift = At_secs - At_secs_CPU;
140  MRPT_LOG_THROTTLE_INFO(10.0,mrpt::format("[HOKUYO::doProcess] Timestamp sync. At_CPU=%e secs. Drift=%e secs.", At_secs_CPU, clock_drift));
141 
143  outObservation.timestamp = m_timeStartTT + AtDO;
144  }
145 
146  // And the scan ranges:
147  outObservation.rightToLeft = true;
148 
149  outObservation.aperture = nRanges * 2 * M_PI / m_sensor_info.scans_per_360deg;
150 
151  outObservation.maxRange = m_sensor_info.d_max;
152  outObservation.stdError = 0.010f;
153  outObservation.sensorPose = m_sensorPose;
154  outObservation.sensorLabel = m_sensorLabel;
155 
156  outObservation.resizeScan(nRanges);
157  char *ptr = (char *)&m_rcv_data[4];
158 
159  if (m_intensity)
160  outObservation.setScanHasIntensity(true);
161 
162  for (int i = 0; i < nRanges; i++) {
163  int b1 = (*ptr++) - 0x30;
164  int b2 = (*ptr++) - 0x30;
165  int b3 = (*ptr++) - 0x30;
166 
167  int range_mm = ((b1 << 12) | (b2 << 6) | b3);
168 
169  outObservation.setScanRange(i, range_mm * 0.001f);
170  outObservation.setScanRangeValidity(
171  i,
172  range_mm >= 20 && (outObservation.scan[i] <= outObservation.maxRange));
173 
174  if (m_intensity) {
175  int b4 = (*ptr++) - 0x30;
176  int b5 = (*ptr++) - 0x30;
177  int b6 = (*ptr++) - 0x30;
178  outObservation.setScanIntensity(i, ((b4 << 12) | (b5 << 6) | b6));
179  }
180  }
181 
182  // Do filter:
185  // Do show preview:
187 
188  outThereIsObservation = true;
189 }
190 
191 /*-------------------------------------------------------------
192  loadConfig_sensorSpecific
193 -------------------------------------------------------------*/
195  const mrpt::utils::CConfigFileBase &configSource,
196  const std::string &iniSection) {
197  m_reduced_fov =
198  DEG2RAD(configSource.read_float(iniSection, "reduced_fov", 0)),
199 
201  configSource.read_int(iniSection, "HOKUYO_motorSpeed_rpm", 0);
203  configSource.read_float(iniSection, "pose_x", 0),
204  configSource.read_float(iniSection, "pose_y", 0),
205  configSource.read_float(iniSection, "pose_z", 0),
206  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0)),
207  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0)),
208  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0)));
209 
211  configSource.read_bool(iniSection, "HOKUYO_HS_mode", m_highSensMode);
212 
213 #ifdef _WIN32
214  m_com_port = configSource.read_string(iniSection, "COM_port_WIN", m_com_port);
215 #else
216  m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port);
217 #endif
218 
219  m_ip_dir = configSource.read_string(iniSection, "IP_DIR", m_ip_dir);
220  m_port_dir = configSource.read_int(iniSection, "PORT_DIR", m_port_dir);
221 
222  ASSERTMSG_(
223  !m_com_port.empty() || !m_ip_dir.empty(),
224  "Either COM_port or IP_DIR must be defined in the configuration file!");
225  ASSERTMSG_(m_com_port.empty() || m_ip_dir.empty(),
226  "Both COM_port and IP_DIR set! Please, define only one of them.");
227  if (!m_ip_dir.empty()) {
229  "A TCP/IP port number `PORT_DIR` must be specified for Ethernet "
230  "connection");
231  }
232 
234  iniSection, "disable_firmware_timestamp", m_disable_firmware_timestamp);
235  m_intensity = configSource.read_bool(iniSection, "intensity", m_intensity),
236 
237  MRPT_LOAD_HERE_CONFIG_VAR(scan_interval, int, m_scan_interval, configSource,
238  iniSection);
239 
240  // Parent options:
241  C2DRangeFinderAbstract::loadCommonParams(configSource, iniSection);
242 }
243 
244 /*-------------------------------------------------------------
245  turnOn
246 -------------------------------------------------------------*/
248  MRPT_START
249 
250  // Bound?
251  if (!ensureStreamIsOpen())
252  return false;
253 
254  // If we are over a serial link, set it up:
255  if (m_ip_dir.empty()) {
256  CSerialPort *COM = dynamic_cast<CSerialPort *>(m_stream);
257 
258  if (COM != nullptr) {
259  // It is a COM:
260  COM->setConfig(19200);
261  COM->setTimeouts(100, 0, 200, 0, 0);
262 
263  // Assure the laser is off and quiet:
264  switchLaserOff();
265  std::this_thread::sleep_for(std::chrono::milliseconds(10));
266 
267  COM->purgeBuffers();
268  std::this_thread::sleep_for(std::chrono::milliseconds(10));
269 
270  COM->setConfig(115200);
271  switchLaserOff();
272  std::this_thread::sleep_for(std::chrono::milliseconds(10));
273  COM->purgeBuffers();
274  std::this_thread::sleep_for(std::chrono::milliseconds(10));
275  COM->setConfig(19200);
276  }
277 
278  if (COM != nullptr) {
279  // Set 115200 baud rate:
280  setHighBaudrate();
281  enableSCIP20();
282  COM->setConfig(115200);
283  }
284  } else {
285  CClientTCPSocket *COM = dynamic_cast<CClientTCPSocket *>(m_stream);
286 
287  if (COM != nullptr) {
288  // Assure the laser is off and quiet:
289  switchLaserOff();
290  std::this_thread::sleep_for(std::chrono::milliseconds(10));
291 
292  purgeBuffers();
293  std::this_thread::sleep_for(std::chrono::milliseconds(10));
294 
295  switchLaserOff();
296  std::this_thread::sleep_for(std::chrono::milliseconds(10));
297  purgeBuffers();
298  }
299  }
300 
301  if (!enableSCIP20())
302  return false;
303 
304  // Turn on the laser:
305  if (!switchLaserOn())
306  return false;
307 
308  // Set the motor speed:
309  if (m_motorSpeed_rpm)
311  return false;
312 
313  // Set HS mode:
315 
316  // Display sensor information:
318  return false;
319 
320  // Set for scanning angles:
323 
324  // Artificially reduced FOV?
325  if (m_reduced_fov > 0 && m_reduced_fov < 2 * M_PI) {
326  int center = (m_lastRange + m_firstRange) >> 1;
327  const int half_range =
328  static_cast<int>((m_sensor_info.scans_per_360deg / 360) *
330  1;
331  m_firstRange = center - half_range;
332  m_lastRange = center + half_range;
333  MRPT_LOG_INFO_STREAM("[HOKUYO::turnOn] Using reduced FOV: ranges ["
334  << m_firstRange << "-" << m_lastRange << "] for "
335  << RAD2DEG(m_reduced_fov) << " deg. FOV");
336  }
337 
338  if (!displayVersionInfo()) {
339  // return false; // It's not SO important
340  }
341 
342  // Start!
343  if (!startScanningMode())
344  return false;
345 
346  return true;
347 
348  MRPT_END
349 }
350 
351 /*-------------------------------------------------------------
352  turnOff
353 -------------------------------------------------------------*/
355  // Turn off the laser:
356  if (!switchLaserOff())
357  return false;
358 
359  return true;
360 }
361 
363  char rcv_status0, rcv_status1;
364  if (!ensureStreamIsOpen())
365  return false;
366 
368  "[CHokuyoURG::setHighBaudrate] Changing baudrate to 115200...");
369 
370  // Send command:
371  sendCmd("SS115200\x0A");
372 
373  // Receive response:
374  if (!receiveResponse(rcv_status0, rcv_status1)) {
375  MRPT_LOG_ERROR("[CHokuyoURG::setHighBaudrate] Error waiting for response");
376  return false;
377  }
378 
379  MRPT_LOG_DEBUG("OK\n");
380  return true;
381 }
382 
383 /*-------------------------------------------------------------
384  assureBufferHasBytes
385 -------------------------------------------------------------*/
386 bool CHokuyoURG::assureBufferHasBytes(const size_t nDesiredBytes) {
387  ASSERT_(nDesiredBytes < m_rx_buffer.capacity());
388 
389  if (m_rx_buffer.size() >= nDesiredBytes) {
390  return true;
391  } else {
392  // Try to read more bytes:
393  uint8_t buf[128];
394  const size_t to_read = std::min(m_rx_buffer.available(), sizeof(buf));
395 
396  try {
397  size_t nRead;
398 
399  if (!m_ip_dir.empty()) {
400  CClientTCPSocket *client = dynamic_cast<CClientTCPSocket *>(m_stream);
401  nRead = client->readAsync(buf, to_read, 100, 10);
402  } else {
403  nRead = m_stream->ReadBuffer(buf, to_read);
404  }
405 
406  m_rx_buffer.push_many(buf, nRead);
407  } catch (std::exception &) {
408  // 0 bytes read
409  }
410 
411  return (m_rx_buffer.size() >= nDesiredBytes);
412  }
413 }
414 
415 bool CHokuyoURG::receiveResponse(char &rcv_status0, char &rcv_status1) {
416  m_rcv_data.clear();
417  if (!ensureStreamIsOpen())
418  return false;
419  ASSERT_(!m_lastSentMeasCmd.empty());
420 
421  try {
422  // Process response:
423  // ---------------------------------
424 
425  // COMMAND ECHO ---------
426  unsigned int i = 0;
427  const unsigned int verifLen = m_lastSentMeasCmd.size();
428 
429  if (verifLen) {
430  do {
431  if (!assureBufferHasBytes(verifLen - i))
432  return false;
433 
434  // If matches the echo, go on:
435  if (m_rx_buffer.pop() == m_lastSentMeasCmd[i])
436  i++;
437  else
438  i = 0;
439  } while (i < verifLen);
440  }
441 
442  // Now, the status bytes:
443  if (!assureBufferHasBytes(2))
444  return false;
445 
446  rcv_status0 = m_rx_buffer.pop();
447  rcv_status1 = m_rx_buffer.pop();
448 
449  // In SCIP2.0, there is an additional sum char:
450  if (rcv_status1 != 0x0A) {
451  // Yes, it is SCIP2.0
452  if (!assureBufferHasBytes(1))
453  return false;
454 
455  // Ignore this byte: sumStatus
456  m_rx_buffer.pop();
457  } else {
458  // Continue, it seems a SCIP1.1 response...
459  }
460 
461  // After the status bytes, there is a LF:
462  if (!assureBufferHasBytes(1))
463  return false;
464  char nextChar = m_rx_buffer.pop();
465  if (nextChar != 0x0A)
466  return false;
467 
468  // -----------------------------------------------------------------------------
469  // Now the data:
470  // There's a problem here, we don't know in advance how many bytes to
471  // read,
472  // so rely on the serial port class implemented buffer and call many
473  // times
474  // the read method with only 1 byte each time:
475  // -----------------------------------------------------------------------------
476  bool lastWasLF = false;
477  i = 0;
478  for (;;) {
479  if (!assureBufferHasBytes(1)) {
480  return false;
481  }
482  m_rcv_data.push_back(m_rx_buffer.pop());
483  i++; // One more byte in the buffer
484 
485  // No data?
486  if (i == 1 && m_rcv_data[0] == 0x0A) {
487  m_rcv_data.clear();
488  return true;
489  }
490 
491  // Is it a LF?
492  if (m_rcv_data[i - 1] == 0x0A) {
493  if (!lastWasLF) {
494  // Discard SUM+LF
495  ASSERT_(i >= 2);
496  i -= 2;
497  m_rcv_data.resize(i);
498  } else {
499  // Discard this last LF:
500  i--;
501 
502  // Done!
503  m_rcv_data.resize(i);
504  MRPT_LOG_DEBUG_STREAM("[Hokuyo] receiveResponse(): RX `" << m_rcv_data
505  << "`");
506 
507  if (rcv_status0 != '0' &&
508  (rcv_status0 != '9' && rcv_status1 != '9')) {
509  MRPT_LOG_ERROR_STREAM("[Hokuyo] Error LIDAR status: "
510  << (int)rcv_status0 << " after command: `"
511  << m_lastSentMeasCmd << "`");
512  return false;
513  }
514 
515  return true;
516  }
517  lastWasLF = true;
518  } else
519  lastWasLF = false;
520  }
521  } catch (std::exception &e) {
522  MRPT_LOG_ERROR_FMT("[Hokuyo] receiveResponse() Exception: %s", e.what());
523  return false;
524  } catch (...) {
525  return false; // Serial port timeout,...
526  }
527 }
528 
530  char rcv_status0, rcv_status1;
531  if (!ensureStreamIsOpen())
532  return false;
533 
534  MRPT_LOG_DEBUG("[CHokuyoURG::enableSCIP20] Changing protocol to SCIP2.0...");
535 
536  // Send command:
537  sendCmd("SCIP2.0\x0A");
538 
539  // Receive response:
540  if (!receiveResponse(rcv_status0, rcv_status1)) {
541  MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
542  return false;
543  }
544 
545  MRPT_LOG_DEBUG("OK\n");
546  return true;
547 }
548 
550  char rcv_status0, rcv_status1;
551 
552  if (!ensureStreamIsOpen())
553  return false;
554 
555  MRPT_LOG_DEBUG("[CHokuyoURG::switchLaserOn] Switching laser ON...");
556 
557  // Send command:
558  sendCmd("BM\x0A");
559 
560  // Receive response:
561  if (!receiveResponse(rcv_status0, rcv_status1)) {
562  MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
563  return false;
564  }
565 
566  MRPT_LOG_DEBUG("OK\n");
567  return true;
568 }
569 
571  char rcv_status0, rcv_status1;
572 
573  if (!ensureStreamIsOpen())
574  return false;
575 
576  MRPT_LOG_DEBUG("[CHokuyoURG::switchLaserOff] Switching laser OFF...");
577 
578  // Send command:
579  sendCmd("QT\x0A");
580 
581  // Receive response:
582  if (!receiveResponse(rcv_status0, rcv_status1)) {
583  MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
584  return false;
585  }
586 
587  MRPT_LOG_DEBUG("OK\n");
588  return true;
589 }
590 
591 void CHokuyoURG::setScanInterval(unsigned int skipScanCount) {
592  m_scan_interval = skipScanCount;
593 }
594 unsigned int CHokuyoURG::getScanInterval() const { return m_scan_interval; }
595 
596 bool CHokuyoURG::setMotorSpeed(int motoSpeed_rpm) {
597  char rcv_status0, rcv_status1;
598  if (!ensureStreamIsOpen())
599  return false;
600 
601  MRPT_LOG_DEBUG_FMT("[CHokuyoURG::setMotorSpeed] Setting to %i rpm...",
602  motoSpeed_rpm);
603 
604  // Send command:
605  int motorSpeedCode = (600 - motoSpeed_rpm) / 6;
606  if (motorSpeedCode < 0 || motorSpeedCode > 10) {
608  << " Motorspeed must be in the range 540-600 rpm");
609  return false;
610  }
611 
612  char cmd[20];
613  os::sprintf(cmd, 20, "CR%02i\x0A", motorSpeedCode);
614  sendCmd(cmd);
615 
616  // Receive response:
617  if (!receiveResponse(rcv_status0, rcv_status1)) {
618  MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
619  return false;
620  }
621 
622  MRPT_LOG_DEBUG("OK\n");
623  return true;
624 }
625 
626 /*-------------------------------------------------------------
627  setHighSensitivityMode
628 -------------------------------------------------------------*/
630  char rcv_status0, rcv_status1;
631  if (!ensureStreamIsOpen())
632  return false;
633 
635  "[CHokuyoURG::setHighSensitivityMode] Setting HS mode to: %s...",
636  enabled ? "true" : "false");
637 
638  // Send command:
639  char cmd[20];
640  os::sprintf(cmd, 20, "HS%i\x0A", enabled ? 1 : 0);
641  sendCmd(cmd);
642 
643  // Receive response:
644  if (!receiveResponse(rcv_status0, rcv_status1)) {
645  MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
646  return false;
647  }
648 
649  MRPT_LOG_DEBUG("OK\n");
650  return true;
651 }
652 
653 /*-------------------------------------------------------------
654  setIntensityMode
655 -------------------------------------------------------------*/
656 bool CHokuyoURG::setIntensityMode(bool enabled) {
657  m_intensity = enabled;
658  return true;
659 }
660 
662  char rcv_status0, rcv_status1;
663  if (!ensureStreamIsOpen())
664  return false;
665 
666  MRPT_LOG_DEBUG("[CHokuyoURG::displayVersionInfo] Asking info...");
667 
668  // Send command:
669  sendCmd("VV\x0A");
670 
671  // Receive response:
672  if (!receiveResponse(rcv_status0, rcv_status1)) {
673  MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
674  return false;
675  }
676 
677  MRPT_LOG_DEBUG("OK\n");
678 
679  // PRINT:
680  for (auto &c : m_rcv_data)
681  if (c == ';')
682  c = '\n';
683  m_rcv_data[m_rcv_data.size()] = '\0';
684 
685  if (!m_rcv_data.empty()) {
687  "\n------------- HOKUYO Scanner: Version Information ------\n"
688  << &m_rcv_data[0]
689  << "\n"
690  "-------------------------------------------------------\n\n");
691  }
692  return true;
693 }
694 
695 /*-------------------------------------------------------------
696  displaySensorInfo
697 -------------------------------------------------------------*/
699  char rcv_status0, rcv_status1;
700  if (!ensureStreamIsOpen())
701  return false;
702 
703  MRPT_LOG_DEBUG("[CHokuyoURG::displaySensorInfo] Asking for info...");
704 
705  // Send command:
706  sendCmd("PP\x0A");
707 
708  // Receive response:
709  if (!receiveResponse(rcv_status0, rcv_status1)) {
710  MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
711  return false;
712  }
713  MRPT_LOG_DEBUG("OK\n");
714 
715  // PRINT:
716  for (auto &c : m_rcv_data)
717  if (c == ';')
718  c = '\n';
719  m_rcv_data[m_rcv_data.size()] = '\0';
720 
721  if (!m_rcv_data.empty()) {
723  "\n------------- HOKUYO Scanner: Product Information ------\n"
724  << &m_rcv_data[0]
725  << "\n"
726  "-------------------------------------------------------\n\n");
727  }
728 
729  // Parse the data:
730  if (out_data) {
731  const char *ptr;
732 
733  if (nullptr != (ptr = strstr(&m_rcv_data[0], "DMAX:")))
734  out_data->d_max = 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], "DMIN:")))
739  out_data->d_min = 0.001 * atoi(ptr + 5);
740  else
741  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
742 
743  if (nullptr != (ptr = strstr(&m_rcv_data[0], "ARES:")))
744  out_data->scans_per_360deg = atoi(ptr + 5);
745  else
746  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
747 
748  if (nullptr != (ptr = strstr(&m_rcv_data[0], "SCAN:")))
749  out_data->motor_speed_rpm = atoi(ptr + 5);
750  else
751  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
752 
753  if (nullptr != (ptr = strstr(&m_rcv_data[0], "AMIN:")))
754  out_data->scan_first = atoi(ptr + 5);
755  else
756  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
757 
758  if (nullptr != (ptr = strstr(&m_rcv_data[0], "AMAX:")))
759  out_data->scan_last = atoi(ptr + 5);
760  else
761  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
762 
763  if (nullptr != (ptr = strstr(&m_rcv_data[0], "AFRT:")))
764  out_data->scan_front = atoi(ptr + 5);
765  else
766  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
767 
768  if (nullptr != (ptr = strstr(&m_rcv_data[0], "MODL:"))) {
769  char aux[30];
770  memcpy(aux, ptr + 5, 8);
771  aux[8] = '\0';
772  out_data->model = aux;
773  } else
774  MRPT_LOG_ERROR("[Hokuyo] displayVersionInfo() parse error");
775  }
776 
777  return true;
778 }
779 
781  char rcv_status0, rcv_status1;
782  if (!ensureStreamIsOpen())
783  return false;
784 
785  MRPT_LOG_DEBUG("[CHokuyoURG::startScanningMode] Starting scanning mode...");
786 
787  // Send command:
788  // 'M' 'D'
789  // 'XXXX' (starting step)
790  // 'XXXX' (end step)
791  // 'XX' (cluster count)
792  // 'X' (scan interval)
793  // 'XX' (number of scans)
794  char cmd[50];
795  unsigned int scan_interval = m_scan_interval;
796  if (scan_interval > 9)
797  scan_interval = 9;
798  os::sprintf(cmd, 50, "M%c%04u%04u01%u00\x0A", m_intensity ? 'E' : 'D',
799  m_firstRange, m_lastRange, scan_interval);
800 
801  sendCmd(cmd);
802 
803  // Receive response:
804  if (!receiveResponse(rcv_status0, rcv_status1)) {
805  MRPT_LOG_ERROR_STREAM(__CURRENT_FUNCTION_NAME__ << ": Error in response");
806  return false;
807  }
808 
809  MRPT_LOG_DEBUG("OK\n");
810  return true;
811 }
812 
814  MRPT_START
815 
816  if (m_stream) {
817  // Socket or USB connection?
818  if (!m_ip_dir.empty() && m_port_dir) {
819  // Has the port been disconected (USB serial ports)??
820  CClientTCPSocket *COM = dynamic_cast<CClientTCPSocket *>(m_stream);
821 
822  if (COM != nullptr) {
823  if (COM->isConnected())
824  return true;
825 
826  // It has been disconnected... try to reconnect:
827  MRPT_LOG_ERROR("[CHokuyoURG] Socket connection lost! trying to "
828  "reconnect...");
829 
830  try {
831  COM->connect(m_ip_dir, m_port_dir);
832  // OK, reconfigure the laser:
833  turnOn();
834  return true;
835  } catch (...) {
836  // Not yet..
837  return false;
838  }
839  } else {
840  return true; // Assume OK
841  }
842  } else {
843  // Has the port been disconected (USB serial ports)??
844  CSerialPort *COM = dynamic_cast<CSerialPort *>(m_stream);
845  if (COM != nullptr) {
846  if (COM->isOpen())
847  return true;
848 
849  // It has been disconnected... try to reconnect:
852  << ": Serial port connection lost! Trying to reconnect...");
853 
854  try {
855  COM->open();
856  // OK, reconfigure the laser:
857  turnOn();
858  return true;
859  } catch (...) {
860  // Not yet..
861  return false;
862  }
863  } else {
864  return true; // Assume OK
865  }
866  }
867  } else {
868  if (m_com_port.empty() && m_ip_dir.empty() && !m_port_dir) {
870  "No stream bound to the laser nor COM serial port or ip and "
871  "port provided in 'm_com_port','m_ip_dir' and 'm_port_dir'");
872  }
873 
874  if (!m_ip_dir.empty()) {
875  // Try to open the serial port:
876  CClientTCPSocket *theCOM = new CClientTCPSocket();
877 
879  << m_ip_dir << ":"
880  << m_port_dir);
881  theCOM->connect(m_ip_dir, m_port_dir);
882 
883  if (!theCOM->isConnected()) {
885  << " Cannot connect with the server '"
886  << m_com_port << "'");
887  delete theCOM;
888  return false;
889  }
890 
891  // Bind:
892  bindIO(theCOM);
893 
895  } else {
896  // Try to open the serial port:
897  CSerialPort *theCOM = new CSerialPort(m_com_port, true);
898 
899  if (!theCOM->isOpen()) {
901  << " Cannot open serial port '" << m_com_port
902  << "'");
903  delete theCOM;
904  return false;
905  }
906 
907  // Bind:
908  bindIO(theCOM);
909 
911  }
912 
913  return true;
914  }
915  MRPT_END
916 }
917 
919  if (m_verbose)
920  this->setMinLoggingLevel(mrpt::utils::LVL_DEBUG);
921 
922  if (!ensureStreamIsOpen())
923  return;
924 
925  if (!turnOn()) {
926  MRPT_LOG_ERROR("[Hokuyo] Error initializing HOKUYO scanner");
927  return;
928  }
929 }
930 
932  if (!ensureStreamIsOpen())
933  return;
934 
935  if (m_ip_dir.empty()) {
936  CSerialPort *COM = dynamic_cast<CSerialPort *>(m_stream);
937  if (COM != nullptr) {
938  COM->purgeBuffers();
939  }
940  } else // Socket connection
941  {
942  CClientTCPSocket *COM = dynamic_cast<CClientTCPSocket *>(m_stream);
943 
944  size_t to_read = COM->getReadPendingBytes();
945 
946  if (to_read) {
947  void *buf = malloc(sizeof(uint8_t) * to_read);
948 
949  size_t nRead = m_stream->ReadBuffer(buf, to_read);
950 
951  if (nRead != to_read)
953  "Error in purge buffers: read and expected number of bytes "
954  "are different.");
955 
956  free(buf);
957  }
958  }
959 }
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.
Definition: CStream.cpp:45
void setScanInterval(unsigned int skipScanCount)
Set the skip scan count (0 means send all scans).
Definition: CHokuyoURG.cpp:591
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
virtual ~CHokuyoURG()
Destructor: turns the laser off.
Definition: CHokuyoURG.cpp:42
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
int m_firstRange
The first and last ranges to consider from the scan.
Definition: CHokuyoURG.h:94
bool isOpen() const
Returns if port has been correctly open.
#define min(a, b)
std::string m_rcv_data
temp buffer for incoming data packets
Definition: CHokuyoURG.h:243
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).
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
void setTimeouts(int ReadIntervalTimeout, int ReadTotalTimeoutMultiplier, int ReadTotalTimeoutConstant, int WriteTotalTimeoutMultiplier, int WriteTotalTimeoutConstant)
Changes the timeouts of the port, in milliseconds.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
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:194
std::string m_sensorLabel
See CGenericSensor.
size_t available() const
The maximum number of elements that can be written ("push") without rising an overflow error...
unsigned int getScanInterval() const
Definition: CHokuyoURG.cpp:594
#define THROW_EXCEPTION(msg)
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
size_t size() const
Return the number of elements available for read ("pop") in the buffer (this is NOT the maximum size ...
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:386
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
Contains classes for various device interfaces.
void setScanHasIntensity(bool setHasIntensityFlag)
Marks this scan as having or not intensity data.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
STL namespace.
#define M_PI
Definition: bits.h:78
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:813
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,...)
#define MRPT_LOG_THROTTLE_INFO(_PERIOD_SECONDS, _STRING)
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:549
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
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
#define MRPT_END
const GLubyte * c
Definition: glext.h:5590
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:656
bool displayVersionInfo()
Ask to the device, and print to the debug stream, details about the firmware version,serial number,...
Definition: CHokuyoURG.cpp:661
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:698
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:415
#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:529
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
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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)...
GLsizei const GLchar ** string
Definition: glext.h:3919
#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:570
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.
#define RAD2DEG
void purgeBuffers()
Empties the RX buffers of the serial port.
Definition: CHokuyoURG.cpp:931
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:780
#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:247
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:596
double d_min
Min/Max ranges, in meters.
Definition: CHokuyoURG.h:83
The namespace for 3D scene representation and rendering.
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)...
#define ASSERT_(f)
size_t capacity() const
Return the maximum capacity of the buffer.
void initialize()
Turns the laser on.
Definition: CHokuyoURG.cpp:918
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:362
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&#39;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
double BASE_IMPEXP timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
Definition: datetime.cpp:205
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:354
#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:629
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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019