MRPT  1.9.9
CVelodyneScanner.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 #include <mrpt/comms/net_utils.h>
17 #include <mrpt/system/datetime.h> // timeDifference
18 #include <mrpt/system/filesystem.h>
19 #include <thread>
20 
21 // socket's hdrs:
22 #ifdef _WIN32
23 #define _WINSOCK_DEPRECATED_NO_WARNINGS
24 #if defined(_WIN32_WINNT) && (_WIN32_WINNT < 0x600)
25 #undef _WIN32_WINNT
26 #define _WIN32_WINNT 0x600 // Minimum: Windows Vista (required to pollfd)
27 #endif
28 
29 #include <winsock2.h>
30 using socklen_t = int;
31 
32 #if defined(_MSC_VER)
33 #pragma comment(lib, "WS2_32.LIB")
34 #endif
35 
36 #else
37 #define INVALID_SOCKET (-1)
38 #include <arpa/inet.h>
39 #include <fcntl.h>
40 #include <netdb.h>
41 #include <netinet/in.h>
42 #include <netinet/tcp.h>
43 #include <poll.h>
44 #include <sys/ioctl.h>
45 #include <sys/socket.h>
46 #include <sys/time.h> // gettimeofday()
47 #include <sys/types.h>
48 #include <unistd.h>
49 #include <cerrno>
50 #endif
51 
52 #if MRPT_HAS_LIBPCAP
53 #include <pcap.h>
54 #if !defined(PCAP_NETMASK_UNKNOWN) // for older pcap versions
55 #define PCAP_NETMASK_UNKNOWN 0xffffffff
56 #endif
57 #endif
58 
59 using namespace mrpt;
60 using namespace mrpt::hwdrivers;
61 using namespace mrpt::obs;
62 using namespace std;
63 
65 
68 
71 {
72  static CVelodyneScanner::model_properties_list_t modelProperties;
73  static bool init = false;
74  if (!init)
75  {
76  init = true;
77  modelProperties[CVelodyneScanner::VLP16].maxRange = 120.0;
78  modelProperties[CVelodyneScanner::HDL32].maxRange = 70.0;
79  modelProperties[CVelodyneScanner::HDL64].maxRange = 120.0;
80  }
81  return modelProperties;
82 }
84 {
87  std::string s;
88  for (auto it : lst)
89  s += mrpt::format(
90  "`%s`,",
92  it.first)
93  .c_str());
94  return s;
95 }
96 
98  : m_device_ip(""),
99 
100  m_last_pos_packet_timestamp(INVALID_TIMESTAMP),
101 
102  m_hDataSock(INVALID_SOCKET),
103  m_hPositionSock(INVALID_SOCKET),
104  m_last_gps_rmc_age(INVALID_TIMESTAMP)
105 
106 {
107  m_sensorLabel = "Velodyne";
108 
109 #if MRPT_HAS_LIBPCAP
110  m_pcap_bpf_program = new bpf_program[1];
111 #endif
112 
113 #ifdef _WIN32
114  // Init the WinSock Library:
115  WORD wVersionRequested = MAKEWORD(2, 0);
116  WSADATA wsaData;
117  if (WSAStartup(wVersionRequested, &wsaData))
118  THROW_EXCEPTION("Error calling WSAStartup");
119 #endif
120 }
121 
123 {
124  this->close();
125 #ifdef _WIN32
126  WSACleanup();
127 #endif
128 
129 #if MRPT_HAS_LIBPCAP
130  delete[] reinterpret_cast<bpf_program*>(m_pcap_bpf_program);
131  m_pcap_bpf_program = nullptr;
132 #endif
133 }
134 
136  const std::string& velodyne_xml_calib_file_path)
137 {
138  return m_velodyne_calib.loadFromXMLFile(velodyne_xml_calib_file_path);
139 }
140 
143 {
144  MRPT_START
145 
147  MRPT_LOAD_HERE_CONFIG_VAR(device_ip, string, m_device_ip, cfg, sect);
148  MRPT_LOAD_HERE_CONFIG_VAR(pcap_input, string, m_pcap_input_file, cfg, sect);
150  pcap_output, string, m_pcap_output_file, cfg, sect);
152  pcap_read_once, bool, m_pcap_read_once, cfg, sect);
154  pcap_read_fast, bool, m_pcap_read_fast, cfg, sect);
156  pcap_read_full_scan_delay_ms, double, m_pcap_read_full_scan_delay_ms,
157  cfg, sect);
159  pcap_repeat_delay, double, m_pcap_repeat_delay, cfg, sect);
161  pos_packets_timing_timeout, double, m_pos_packets_timing_timeout, cfg,
162  sect);
164  pos_packets_min_period, double, m_pos_packets_min_period, cfg, sect);
165 
166  using mrpt::DEG2RAD;
168  cfg.read_float(sect, "pose_x", 0), cfg.read_float(sect, "pose_y", 0),
169  cfg.read_float(sect, "pose_z", 0),
170  DEG2RAD(cfg.read_float(sect, "pose_yaw", 0)),
171  DEG2RAD(cfg.read_float(sect, "pose_pitch", 0)),
172  DEG2RAD(cfg.read_float(sect, "pose_roll", 0)));
173 
174  std::string calibration_file;
175  MRPT_LOAD_CONFIG_VAR(calibration_file, string, cfg, sect);
176  if (!calibration_file.empty()) this->loadCalibrationFile(calibration_file);
177 
178  // Check validity:
180  if (lstModels.find(m_model) == lstModels.end())
181  {
183  "Unrecognized `model` parameter: `%u` . Known values are: %s",
184  static_cast<unsigned int>(m_model),
186  }
187 
188  // Optional HTTP-based settings:
189  MRPT_LOAD_HERE_CONFIG_VAR(rpm, int, m_lidar_rpm, cfg, sect);
191  cfg.read_enum<return_type_t>(sect, "return_type", m_lidar_return);
192 
193  MRPT_END
194 }
195 
199 {
200  try
201  {
202  ASSERTMSG_(m_initialized, "initialize() has not been called yet!");
203 
204  // Init with empty smart pointers:
207 
208  // Try to get data & pos packets:
211  mrpt::system::TTimeStamp data_pkt_timestamp, pos_pkt_timestamp;
212  bool rx_all_ok = this->receivePackets(
213  data_pkt_timestamp, rx_pkt, pos_pkt_timestamp, rx_pos_pkt);
214 
215  if (!rx_all_ok)
216  {
217  // PCAP EOF:
218  return false;
219  }
220 
221  if (pos_pkt_timestamp != INVALID_TIMESTAMP)
222  {
225  gps_obs->sensorLabel = this->m_sensorLabel + std::string("_GPS");
226  gps_obs->sensorPose = m_sensorPose;
227 
228  gps_obs->originalReceivedTimestamp = pos_pkt_timestamp;
229 
230  bool parsed_ok = CGPSInterface::parse_NMEA(
231  std::string(rx_pos_pkt.NMEA_GPRMC), *gps_obs);
232  const mrpt::obs::gnss::Message_NMEA_RMC* msg_rmc =
233  gps_obs->getMsgByClassPtr<mrpt::obs::gnss::Message_NMEA_RMC>();
234  if (!parsed_ok || !msg_rmc || msg_rmc->fields.validity_char != 'A')
235  {
236  gps_obs->has_satellite_timestamp = false;
237  gps_obs->timestamp = pos_pkt_timestamp;
238  }
239  else
240  {
241  // We have live GPS signal and a recent RMC frame:
242  m_last_gps_rmc_age = pos_pkt_timestamp;
243  m_last_gps_rmc = *msg_rmc;
244  }
245  outGPS = gps_obs; // save in output object
246  }
247 
248  if (data_pkt_timestamp != INVALID_TIMESTAMP)
249  {
250  m_state = ssWorking;
251 
252  // Break into a new observation object when the azimuth passes
253  // 360->0 deg:
254  const uint16_t rx_pkt_start_angle = rx_pkt.blocks[0].rotation;
255  // const uint16_t rx_pkt_end_angle =
256  // rx_pkt.blocks[CObservationVelodyneScan::BLOCKS_PER_PACKET-1].rotation;
257 
258  // Return the observation as done when a complete 360 deg scan is
259  // ready:
260  if (m_rx_scan && !m_rx_scan->scan_packets.empty())
261  {
262  if ((rx_pkt_start_angle <
263  m_rx_scan->scan_packets.rbegin()->blocks[0].rotation) ||
265  {
266  outScan = m_rx_scan;
267  m_rx_scan.reset();
268 
269  if (m_pcap)
270  {
271  // Keep the reader from blowing through the file.
272  if (!m_pcap_read_fast)
273  std::this_thread::sleep_for(
274  std::chrono::duration<double, std::milli>(
276  }
277  }
278  }
279 
280  // Create smart ptr to new in-progress observation:
281  if (!m_rx_scan)
282  {
284  m_rx_scan->sensorLabel =
285  this->m_sensorLabel + std::string("_SCAN");
286  m_rx_scan->sensorPose = m_sensorPose;
287  m_rx_scan->calibration =
288  m_velodyne_calib; // Embed a copy of the calibration info
289 
290  {
291  const model_properties_list_t& lstModels =
293  auto it = lstModels.find(this->m_model);
294  if (it != lstModels.end())
295  { // Model params:
296  m_rx_scan->maxRange = it->second.maxRange;
297  }
298  else // default params:
299  {
300  m_rx_scan->maxRange = 120.0;
301  }
302  }
303  }
304 
305  // For the first packet, set timestamp:
306  if (m_rx_scan->scan_packets.empty())
307  {
308  m_rx_scan->originalReceivedTimestamp = data_pkt_timestamp;
309  // Using GPS, if available:
310  if (m_last_gps_rmc.fields.validity_char == 'A' &&
312  m_last_gps_rmc_age, data_pkt_timestamp) <
314  {
315  // Each Velodyne data packet has a timestamp field,
316  // with the number of us since the top of the current HOUR:
317  // take the date and time from the GPS, then modify minutes
318  // and seconds from data pkt:
319  const mrpt::system::TTimeStamp gps_tim =
322 
323  mrpt::system::TTimeParts tim_parts;
324  mrpt::system::timestampToParts(gps_tim, tim_parts);
325  tim_parts.minute =
326  rx_pkt.gps_timestamp /*us from top of hour*/ /
327  60000000ul;
328  tim_parts.second =
329  (rx_pkt.gps_timestamp /*us from top of hour*/ %
330  60000000ul) *
331  1e-6;
332 
333  const mrpt::system::TTimeStamp data_pkt_tim =
335 
336  m_rx_scan->timestamp = data_pkt_tim;
337  m_rx_scan->has_satellite_timestamp = true;
338  }
339  else
340  {
341  m_rx_scan->has_satellite_timestamp = false;
342  m_rx_scan->timestamp = data_pkt_timestamp;
343  }
344  }
345 
346  // Accumulate pkts in the observation object:
347  m_rx_scan->scan_packets.push_back(rx_pkt);
348  }
349 
350  return true;
351  }
352  catch (exception& e)
353  {
354  cerr << "[CVelodyneScanner::getObservation] Returning false due to "
355  "exception: "
356  << endl;
357  cerr << e.what() << endl;
358  return false;
359  }
360 }
361 
363 {
365  CObservationGPS::Ptr obs_gps;
366 
367  if (getNextObservation(obs, obs_gps))
368  {
369  m_state = ssWorking;
370  if (obs) appendObservation(obs);
371  if (obs_gps) appendObservation(obs_gps);
372  }
373  else
374  {
375  m_state = ssError;
376  cerr << "ERROR receiving data from Velodyne devic!" << endl;
377  }
378 }
379 
380 /** Tries to initialize the sensor, after setting all the parameters with a call
381  * to loadConfig.
382  * \exception This method must throw an exception with a descriptive message if
383  * some critical error is found. */
385 {
386  this->close();
387 
388  // (0) Preparation:
389  // --------------------------------
390  // Make sure we have calibration data:
391  if (m_velodyne_calib.empty())
392  {
393  // Try to load default data:
394  m_velodyne_calib = VelodyneCalibration::LoadDefaultCalibration(
396  m_model));
397  if (m_velodyne_calib.empty())
399  "Could not find default calibration data for the given LIDAR "
400  "`model` name. Please, specify a valid `model` or load a valid "
401  "XML configuration file first.");
402  }
403 
404  // online vs off line operation??
405  // -------------------------------
406  if (m_pcap_input_file.empty())
407  { // Online
408 
409  if (m_lidar_rpm > 0)
410  {
411  if (!setLidarRPM(m_lidar_rpm))
412  THROW_EXCEPTION("Error in setLidarRPM();");
413  }
414  if (m_lidar_return != UNCHANGED)
415  {
417  THROW_EXCEPTION("Error in setLidarReturnType();");
418  }
419 
420  // (1) Create LIDAR DATA socket
421  // --------------------------------
422  if (INVALID_SOCKET == (m_hDataSock = socket(PF_INET, SOCK_DGRAM, 0)))
424  "Error creating UDP socket:\n%s",
426 
427  struct sockaddr_in bindAddr;
428  memset(&bindAddr, 0, sizeof(bindAddr));
429  bindAddr.sin_family = AF_INET;
430  bindAddr.sin_port = htons(VELODYNE_DATA_UDP_PORT);
431  bindAddr.sin_addr.s_addr = INADDR_ANY;
432 
433  if (int(INVALID_SOCKET) ==
434  ::bind(
435  m_hDataSock, (struct sockaddr*)(&bindAddr), sizeof(sockaddr)))
437 
438 #ifdef _WIN32
439  unsigned long non_block_mode = 1;
440  if (ioctlsocket(m_hDataSock, FIONBIO, &non_block_mode))
442  "Error entering non-blocking mode with ioctlsocket().");
443 #else
444  int oldflags = fcntl(m_hDataSock, F_GETFL, 0);
445  if (oldflags == -1)
446  THROW_EXCEPTION("Error retrieving fcntl() of socket.");
447  oldflags |= O_NONBLOCK | FASYNC;
448  if (-1 == fcntl(m_hDataSock, F_SETFL, oldflags))
449  THROW_EXCEPTION("Error entering non-blocking mode with fcntl();");
450 #endif
451 
452  // (2) Create LIDAR POSITION socket
453  // --------------------------------
454  if (INVALID_SOCKET ==
455  (m_hPositionSock = socket(PF_INET, SOCK_DGRAM, 0)))
457  "Error creating UDP socket:\n%s",
459 
460  bindAddr.sin_port = htons(VELODYNE_POSITION_UDP_PORT);
461 
462  if (int(INVALID_SOCKET) == ::bind(
464  (struct sockaddr*)(&bindAddr),
465  sizeof(sockaddr)))
467 
468 #ifdef _WIN32
469  if (ioctlsocket(m_hPositionSock, FIONBIO, &non_block_mode))
471  "Error entering non-blocking mode with ioctlsocket().");
472 #else
473  oldflags = fcntl(m_hPositionSock, F_GETFL, 0);
474  if (oldflags == -1)
475  THROW_EXCEPTION("Error retrieving fcntl() of socket.");
476  oldflags |= O_NONBLOCK | FASYNC;
477  if (-1 == fcntl(m_hPositionSock, F_SETFL, oldflags))
478  THROW_EXCEPTION("Error entering non-blocking mode with fcntl();");
479 #endif
480  }
481  else
482  { // Offline:
483 #if MRPT_HAS_LIBPCAP
484  char errbuf[PCAP_ERRBUF_SIZE];
485 
486  if (m_pcap_verbose)
487  printf(
488  "\n[CVelodyneScanner] Opening PCAP file \"%s\"\n",
489  m_pcap_input_file.c_str());
490  if ((m_pcap = pcap_open_offline(m_pcap_input_file.c_str(), errbuf)) ==
491  nullptr)
492  {
493  THROW_EXCEPTION_FMT("Error opening PCAP file: '%s'", errbuf);
494  }
495 
496  // Build PCAP filter:
497  {
498  std::string filter_str = mrpt::format(
499  "(udp dst port %d || udp dst port %d)",
502  if (!m_device_ip.empty())
503  filter_str += "&& src host " + m_device_ip;
504 
505  static std::string sMsgError =
506  "[CVelodyneScanner] Error calling pcap_compile: "; // This is
507  // to avoid
508  // the
509  // ill-formed
510  // signature
511  // of
512  // pcap_error()
513  // accepting
514  // "char*",
515  // not
516  // "const
517  // char*"...
518  // sigh
519  if (pcap_compile(
520  reinterpret_cast<pcap_t*>(m_pcap),
521  reinterpret_cast<bpf_program*>(m_pcap_bpf_program),
522  filter_str.c_str(), 1, PCAP_NETMASK_UNKNOWN) < 0)
523  pcap_perror(reinterpret_cast<pcap_t*>(m_pcap), &sMsgError[0]);
524  }
525 
526  m_pcap_file_empty = true;
527  m_pcap_read_count = 0;
528 
529 #else
531  "Velodyne: Reading from PCAP requires building MRPT with libpcap "
532  "support!");
533 #endif
534  }
535 
539 
540  m_initialized = true;
541 }
542 
544 {
545  if (m_hDataSock != INVALID_SOCKET)
546  {
547  shutdown(m_hDataSock, 2); // SD_BOTH );
548 #ifdef _WIN32
549  closesocket(m_hDataSock);
550 #else
552 #endif
553  m_hDataSock = INVALID_SOCKET;
554  }
555 
556  if (m_hPositionSock != INVALID_SOCKET)
557  {
558  shutdown(m_hPositionSock, 2); // SD_BOTH );
559 #ifdef _WIN32
560  closesocket(m_hPositionSock);
561 #else
563 #endif
564  m_hPositionSock = INVALID_SOCKET;
565  }
566 #if MRPT_HAS_LIBPCAP
567  if (m_pcap)
568  {
569  pcap_close(reinterpret_cast<pcap_t*>(m_pcap));
570  m_pcap = nullptr;
571  }
572  if (m_pcap_dumper)
573  {
574  pcap_dump_close(reinterpret_cast<pcap_dumper_t*>(m_pcap_dumper));
575  m_pcap_dumper = nullptr;
576  }
577  if (m_pcap_out)
578  {
579  pcap_close(reinterpret_cast<pcap_t*>(m_pcap_out));
580  m_pcap_out = nullptr;
581  }
582 #endif
583  m_initialized = false;
584 }
585 
586 // Fixed Ethernet headers for PCAP capture --------
587 #if MRPT_HAS_LIBPCAP
588 const uint16_t LidarPacketHeader[21] = {0xffff, 0xffff, 0xffff, 0x7660, 0x0088,
589  0x0000, 0x0008, 0x0045, 0xd204, 0x0000,
590  0x0040, 0x11ff, 0xaab4, 0xa8c0, 0xc801,
591  0xffff, // checksum 0xa9b4 //source ip
592  // 0xa8c0, 0xc801 is
593  // 192.168.1.200
594  0xffff, 0x4009, 0x4009, 0xbe04, 0x0000};
595 const uint16_t PositionPacketHeader[21] = {
596  0xffff, 0xffff, 0xffff, 0x7660, 0x0088, 0x0000, 0x0008, 0x0045, 0xd204,
597  0x0000, 0x0040, 0x11ff, 0xaab4, 0xa8c0, 0xc801, 0xffff, // checksum 0xa9b4
598  // //source ip
599  // 0xa8c0, 0xc801
600  // is 192.168.1.200
601  0xffff, 0x7420, 0x7420, 0x0802, 0x0000};
602 #endif
603 
604 #if defined(_WIN32) && MRPT_HAS_LIBPCAP
605 int gettimeofday(struct timeval* tp, void*)
606 {
607  FILETIME ft;
608  ::GetSystemTimeAsFileTime(&ft);
609  long long t =
610  (static_cast<long long>(ft.dwHighDateTime) << 32) | ft.dwLowDateTime;
611  t -= 116444736000000000LL;
612  t /= 10; // microseconds
613  tp->tv_sec = static_cast<long>(t / 1000000UL);
614  tp->tv_usec = static_cast<long>(t % 1000000UL);
615  return 0;
616 }
617 #endif
618 
620  mrpt::system::TTimeStamp& data_pkt_timestamp,
622  mrpt::system::TTimeStamp& pos_pkt_timestamp,
624 {
625  static_assert(
627  CObservationVelodyneScan::POS_PACKET_SIZE,
628  "Velodyne pos packet: wrong size!");
629  static_assert(
631  CObservationVelodyneScan::PACKET_SIZE,
632  "Velodyne raw packet: wrong size!");
633 
634  bool ret = true; // all ok
635  if (m_pcap)
636  {
638  data_pkt_timestamp, (uint8_t*)&out_data_pkt, pos_pkt_timestamp,
639  (uint8_t*)&out_pos_pkt);
640  }
641  else
642  {
643  data_pkt_timestamp = internal_receive_UDP_packet(
644  m_hDataSock, (uint8_t*)&out_data_pkt,
645  CObservationVelodyneScan::PACKET_SIZE, m_device_ip);
646  pos_pkt_timestamp = internal_receive_UDP_packet(
647  m_hPositionSock, (uint8_t*)&out_pos_pkt,
648  CObservationVelodyneScan::POS_PACKET_SIZE, m_device_ip);
649  }
650 
651 // Optional PCAP dump:
652 #if MRPT_HAS_LIBPCAP
653  // Save to PCAP file?
654  if (!m_pcap_output_file.empty() &&
655  !m_pcap_out) // 1st time: create output file
656  {
657 #if MRPT_HAS_LIBPCAP
658  char errbuf[PCAP_ERRBUF_SIZE];
659 
662  string sFilePostfix = "_";
663  sFilePostfix += format(
664  "%04u-%02u-%02u_%02uh%02um%02us", (unsigned int)parts.year,
665  (unsigned int)parts.month, (unsigned int)parts.day,
666  (unsigned int)parts.hour, (unsigned int)parts.minute,
667  (unsigned int)parts.second);
668  const string sFileName =
671  string(".pcap");
672 
673  m_pcap_out = pcap_open_dead(DLT_EN10MB, 65535);
674  ASSERTMSG_(
675  m_pcap_out != nullptr, "Error creating PCAP live capture handle");
676 
677  printf(
678  "\n[CVelodyneScanner] Writing to PCAP file \"%s\"\n",
679  sFileName.c_str());
680  if ((m_pcap_dumper = pcap_dump_open(
681  reinterpret_cast<pcap_t*>(m_pcap_out), sFileName.c_str())) ==
682  nullptr)
683  {
685  "Error creating PCAP live dumper: '%s'", errbuf);
686  }
687 #else
689  "Velodyne: Writing PCAP files requires building MRPT with libpcap "
690  "support!");
691 #endif
692  }
693 
694  if (m_pcap_out && m_pcap_dumper &&
695  (data_pkt_timestamp != INVALID_TIMESTAMP ||
696  pos_pkt_timestamp != INVALID_TIMESTAMP))
697  {
698  struct pcap_pkthdr header;
699  struct timeval currentTime;
700  gettimeofday(&currentTime, nullptr);
701  std::vector<unsigned char> packetBuffer;
702 
703  // Data pkt:
704  if (data_pkt_timestamp != INVALID_TIMESTAMP)
705  {
706  header.caplen = header.len =
707  CObservationVelodyneScan::PACKET_SIZE + 42;
708  packetBuffer.resize(header.caplen);
709  header.ts = currentTime;
710 
711  memcpy(&(packetBuffer[0]), LidarPacketHeader, 42);
712  memcpy(
713  &(packetBuffer[0]) + 42, (uint8_t*)&out_data_pkt,
714  CObservationVelodyneScan::PACKET_SIZE);
715  pcap_dump(
716  (u_char*)this->m_pcap_dumper, &header, &(packetBuffer[0]));
717  }
718  // Pos pkt:
719  if (pos_pkt_timestamp != INVALID_TIMESTAMP)
720  {
721  header.caplen = header.len =
722  CObservationVelodyneScan::POS_PACKET_SIZE + 42;
723  packetBuffer.resize(header.caplen);
724  header.ts = currentTime;
725 
726  memcpy(&(packetBuffer[0]), PositionPacketHeader, 42);
727  memcpy(
728  &(packetBuffer[0]) + 42, (uint8_t*)&out_pos_pkt,
729  CObservationVelodyneScan::POS_PACKET_SIZE);
730  pcap_dump(
731  (u_char*)this->m_pcap_dumper, &header, &(packetBuffer[0]));
732  }
733  }
734 #endif
735 
736 // Convert from Velodyne's standard little-endian ordering to host byte
737 // ordering:
738 // (done AFTER saving the pckg as is to pcap above)
739 #if MRPT_IS_BIG_ENDIAN
740  if (data_pkt_timestamp != INVALID_TIMESTAMP)
741  {
743  for (int i = 0; i < CObservationVelodyneScan::BLOCKS_PER_PACKET; i++)
744  {
745  mrpt::reverseBytesInPlace(out_data_pkt.blocks[i].header);
746  mrpt::reverseBytesInPlace(out_data_pkt.blocks[i].rotation);
747  for (int k = 0; k < CObservationVelodyneScan::SCANS_PER_BLOCK; k++)
748  {
750  out_data_pkt.blocks[i].laser_returns[k].distance);
751  }
752  }
753  }
754  if (pos_pkt_timestamp != INVALID_TIMESTAMP)
755  {
757  mrpt::reverseBytesInPlace(out_pos_pkt.unused2);
758  }
759 #endif
760 
761  // Position packet decimation:
762  if (pos_pkt_timestamp != INVALID_TIMESTAMP)
763  {
766  m_last_pos_packet_timestamp, pos_pkt_timestamp) <
768  {
769  // Ignore this packet
770  pos_pkt_timestamp = INVALID_TIMESTAMP;
771  }
772  else
773  {
774  // Reset time watch:
775  m_last_pos_packet_timestamp = pos_pkt_timestamp;
776  }
777  }
778 
779  return ret;
780 }
781 
782 // static method:
784  platform_socket_t hSocket, uint8_t* out_buffer,
785  const size_t expected_packet_size, const std::string& filter_only_from_IP)
786 {
787  if (hSocket == INVALID_SOCKET)
789  "Error: UDP socket is not open yet! Have you called initialize() "
790  "first?");
791 
792  unsigned long devip_addr = 0;
793  if (!filter_only_from_IP.empty())
794  devip_addr = inet_addr(filter_only_from_IP.c_str());
795 
797 
798  struct pollfd fds[1];
799  fds[0].fd = hSocket;
800  fds[0].events = POLLIN;
801  static const int POLL_TIMEOUT = 1; // (ms)
802 
803  sockaddr_in sender_address;
804  socklen_t sender_address_len = sizeof(sender_address);
805 
806  while (true)
807  {
808  // Unfortunately, the Linux kernel recvfrom() implementation
809  // uses a non-interruptible std::this_thread::sleep_for(ms) when waiting
810  // for data,
811  // which would cause this method to hang if the device is not
812  // providing data. We poll() the device first to make sure
813  // the recvfrom() will not block.
814  //
815  // Note, however, that there is a known Linux kernel bug:
816  //
817  // Under Linux, select() may report a socket file descriptor
818  // as "ready for reading", while nevertheless a subsequent
819  // read blocks. This could for example happen when data has
820  // arrived but upon examination has wrong checksum and is
821  // discarded. There may be other circumstances in which a
822  // file descriptor is spuriously reported as ready. Thus it
823  // may be safer to use O_NONBLOCK on sockets that should not
824  // block.
825 
826  // poll() until input available
827  do
828  {
829  int retval =
830 #if !defined(_WIN32)
831  poll
832 #else
833  WSAPoll
834 #endif
835  (fds, 1, POLL_TIMEOUT);
836  if (retval < 0) // poll() error?
837  {
838  if (errno != EINTR)
840  "Error in UDP poll():\n%s",
842  }
843  if (retval == 0) // poll() timeout?
844  {
845  return INVALID_TIMESTAMP;
846  }
847  if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) ||
848  (fds[0].revents & POLLNVAL)) // device error?
849  {
851  "Error in UDP poll() (seems Velodyne device error?)");
852  }
853  } while ((fds[0].revents & POLLIN) == 0);
854 
855  // Receive packets that should now be available from the
856  // socket using a blocking read.
857  int nbytes = recvfrom(
858  hSocket, (char*)&out_buffer[0], expected_packet_size, 0,
859  (sockaddr*)&sender_address, &sender_address_len);
860 
861  if (nbytes < 0)
862  {
863  if (errno != EWOULDBLOCK) THROW_EXCEPTION("recvfrom() failed!?!");
864  }
865  else if ((size_t)nbytes == expected_packet_size)
866  {
867  // read successful,
868  // if packet is not from the lidar scanner we selected by IP,
869  // continue otherwise we are done
870  if (!filter_only_from_IP.empty() &&
871  sender_address.sin_addr.s_addr != devip_addr)
872  continue;
873  else
874  break; // done
875  }
876 
877  std::cerr
878  << "[CVelodyneScanner] Warning: incomplete Velodyne packet read: "
879  << nbytes << " bytes\n";
880  }
881 
882  // Average the times at which we begin and end reading. Use that to
883  // estimate when the scan occurred.
885 
887  time1.time_since_epoch().count() / 2 +
888  time2.time_since_epoch().count() / 2));
889 }
890 
892  mrpt::system::TTimeStamp& data_pkt_time, uint8_t* out_data_buffer,
893  mrpt::system::TTimeStamp& pos_pkt_time, uint8_t* out_pos_buffer)
894 {
895 #if MRPT_HAS_LIBPCAP
896  ASSERT_(m_pcap);
897 
898  data_pkt_time = INVALID_TIMESTAMP;
899  pos_pkt_time = INVALID_TIMESTAMP;
900 
901  char errbuf[PCAP_ERRBUF_SIZE];
902  struct pcap_pkthdr* header;
903  const u_char* pkt_data;
904 
905  while (true)
906  {
907  int res;
908  if ((res = pcap_next_ex(
909  reinterpret_cast<pcap_t*>(m_pcap), &header, &pkt_data)) >= 0)
910  {
912 
913  // if packet is not from the lidar scanner we selected by IP,
914  // continue
915  if (pcap_offline_filter(
916  reinterpret_cast<bpf_program*>(m_pcap_bpf_program), header,
917  pkt_data) == 0)
918  {
919  if (m_verbose)
920  std::cout
921  << "[CVelodyneScanner] DEBUG: Filtering out packet #"
922  << m_pcap_read_count << " in PCAP file.\n";
923  continue;
924  }
925 
926  // Determine whether it is a DATA or POSITION packet:
927  m_pcap_file_empty = false;
929  const uint16_t udp_dst_port =
930  ntohs(*reinterpret_cast<const uint16_t*>(pkt_data + 0x24));
931 
933  {
934  if (m_verbose)
935  std::cout << "[CVelodyneScanner] DEBUG: Packet #"
937  << " in PCAP file is POSITION pkt.\n";
938  memcpy(
939  out_pos_buffer, pkt_data + 42,
940  CObservationVelodyneScan::POS_PACKET_SIZE);
941  pos_pkt_time = tim; // success
942  return true;
943  }
944  else if (udp_dst_port == CVelodyneScanner::VELODYNE_DATA_UDP_PORT)
945  {
946  if (m_verbose)
947  std::cout << "[CVelodyneScanner] DEBUG: Packet #"
949  << " in PCAP file is DATA pkt.\n";
950  memcpy(
951  out_data_buffer, pkt_data + 42,
952  CObservationVelodyneScan::PACKET_SIZE);
953  data_pkt_time = tim; // success
954  return true;
955  }
956  else
957  {
958  std::cerr << "[CVelodyneScanner] ERROR: Packet "
960  << " in PCAP file passed the filter but does not "
961  "match expected UDP port numbers! Skipping it.\n";
962  }
963  }
964 
965  if (m_pcap_file_empty) // no data in file?
966  {
967  fprintf(
968  stderr,
969  "[CVelodyneScanner] Maybe the PCAP file is empty? Error %d "
970  "reading Velodyne packet: `%s`\n",
971  res, pcap_geterr(reinterpret_cast<pcap_t*>(m_pcap)));
972  return true;
973  }
974 
975  if (m_pcap_read_once)
976  {
977  if (m_pcap_verbose)
978  printf(
979  "[CVelodyneScanner] INFO: end of file reached -- done "
980  "reading.\n");
981  std::this_thread::sleep_for(250ms);
982  return false;
983  }
984 
985  if (m_pcap_repeat_delay > 0.0)
986  {
987  if (m_pcap_verbose)
988  printf(
989  "[CVelodyneScanner] INFO: end of file reached -- delaying "
990  "%.3f seconds.\n",
992  std::this_thread::sleep_for(
993  std::chrono::duration<double, std::milli>(
994  m_pcap_repeat_delay * 1000.0));
995  }
996 
997  if (m_pcap_verbose)
998  printf("[CVelodyneScanner] INFO: replaying Velodyne dump file.\n");
999 
1000  // rewind the file
1001  pcap_close(reinterpret_cast<pcap_t*>(m_pcap));
1002  if ((m_pcap = pcap_open_offline(m_pcap_input_file.c_str(), errbuf)) ==
1003  nullptr)
1004  {
1005  THROW_EXCEPTION_FMT("Error opening PCAP file: '%s'", errbuf);
1006  }
1007  m_pcap_file_empty = true; // maybe the file disappeared?
1008  } // loop back and try again
1009 #else
1011  "MRPT needs to be built against libpcap to enable this functionality");
1012 #endif
1013 }
1014 
1016 {
1017  /* HTTP-based config: http://10.0.0.100/tab/config.html
1018  <form name='returns' method="post" action="/cgi/setting">
1019  <span>Return&nbsp;Type:&nbsp;</span> <select name="returns"
1020  onchange='javascript:this.form.submit()'>
1021  <option>Strongest</option>
1022  <option>Last</option>
1023  <option>Dual</option>
1024  </select>
1025  </form>
1026  */
1027  MRPT_START;
1028  std::string strRet;
1029  switch (ret_type)
1030  {
1031  case STRONGEST:
1032  strRet = "Strongest";
1033  break;
1034  case DUAL:
1035  strRet = "Dual";
1036  break;
1037  case LAST:
1038  strRet = "Last";
1039  break;
1040  case UNCHANGED:
1041  return true;
1042  default:
1043  THROW_EXCEPTION("Invalid value for return type!");
1044  };
1045 
1046  const std::string cmd = mrpt::format("returns=%s", strRet.c_str());
1047  return this->internal_send_http_post(cmd);
1048  MRPT_END;
1049 }
1050 
1052 {
1053  /* HTTP-based config: http://10.0.0.100/tab/config.html
1054  <form name='rpm' method="post" action="/cgi/setting">
1055  Motor
1056  &nbsp;RPM:&nbsp;<input type="text" name="rpm" size="5"
1057  style="text-align:right" /><input type="button" value="+"
1058  onclick="javascript:this.form.rpm.value++;this.form.submit()" /><input
1059  type="button" value="-"
1060  onclick="javascript:this.form.rpm.value--;this.form.submit()" />
1061  &nbsp;<input type="submit" value="Set" />
1062  </form>
1063  */
1064 
1065  MRPT_START;
1066  const std::string cmd = mrpt::format("rpm=%i", rpm);
1067  return this->internal_send_http_post(cmd);
1068  MRPT_END;
1069 }
1070 
1072 {
1073  // laser = on|off
1074  MRPT_START;
1075  const std::string cmd = mrpt::format("laser=%s", on ? "on" : "off");
1076  return this->internal_send_http_post(cmd);
1077  MRPT_END;
1078 }
1079 
1081 {
1082  // frame publishing | data packet publishing = on|off
1083  MRPT_START;
1084  m_return_frames = on;
1085  MRPT_END;
1086 }
1087 
1089 {
1090  MRPT_START;
1091 
1092  ASSERTMSG_(
1093  !m_device_ip.empty(), "A device IP address must be specified first!");
1094 
1095  using namespace mrpt::comms::net;
1096 
1097  std::vector<uint8_t> post_out;
1098  string post_err_str;
1099 
1100  int http_rep_code;
1101  mrpt::system::TParameters<string> extra_headers, out_headers;
1102 
1103  extra_headers["Origin"] = mrpt::format("http://%s", m_device_ip.c_str());
1104  extra_headers["Referer"] = mrpt::format("http://%s", m_device_ip.c_str());
1105  extra_headers["Upgrade-Insecure-Requests"] = "1";
1106  extra_headers["Content-Type"] = "application/x-www-form-urlencoded";
1107 
1109  "POST", post_data,
1110  mrpt::format("http://%s/cgi/setting", m_device_ip.c_str()), post_out,
1111  post_err_str, 80 /* port */, string(), string(), // user,pass
1112  &http_rep_code, &extra_headers, &out_headers);
1113 
1114  return mrpt::comms::net::erOk == ret &&
1115  (http_rep_code == 200 || http_rep_code == 204); // OK codes
1116 
1117  MRPT_END;
1118 }
std::chrono::duration< rep, period > duration
Definition: Clock.h:24
void timestampToParts(TTimeStamp t, TTimeParts &p, bool localTime=false)
Gets the individual parts of a date/time (days, hours, minutes, seconds) - UTC time or local time...
Definition: datetime.cpp:50
content_t fields
Message content, accesible by individual fields.
bool loadFromXMLFile(const std::string &velodyne_calibration_xml_filename)
Loads calibration from file, in the format supplied by the manufacturer.
#define MRPT_START
Definition: exceptions.h:241
GLdouble GLdouble t
Definition: glext.h:3695
A C++ interface to Velodyne laser scanners (HDL-64, HDL-32, VLP-16), working on Linux and Windows...
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
mrpt::system::TTimeStamp getAsTimestamp(const mrpt::system::TTimeStamp &date) const
Build an MRPT timestamp with the hour/minute/sec of this structure and the date from the given timest...
static short int VELODYNE_POSITION_UDP_PORT
Default: 8308.
std::map< model_t, TModelProperties > model_properties_list_t
std::chrono::time_point< Clock > time_point
Definition: Clock.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
std::string m_sensorLabel
See CGenericSensor.
bool getNextObservation(mrpt::obs::CObservationVelodyneScan::Ptr &outScan, mrpt::obs::CObservationGPS::Ptr &outGPS)
Polls the UDP port for incoming data packets.
bool internal_read_PCAP_packet(mrpt::system::TTimeStamp &data_pkt_time, uint8_t *out_data_buffer, mrpt::system::TTimeStamp &pos_pkt_time, uint8_t *out_pos_buffer)
mrpt::system::TTimeStamp buildTimestampFromParts(const mrpt::system::TTimeParts &p)
Builds a timestamp from the parts (Parts are in UTC)
Definition: datetime.cpp:74
bool m_return_frames
Default: true Output whole frames and not data packets.
static std::string getListKnownModels()
Return human-readable string: "`VLP16`,`XXX`,...".
std::string getLastSocketErrorStr()
Returns a description of the last Sockets error.
Definition: net_utils.cpp:480
void close()
Close the UDP sockets set-up in initialize().
bool m_pcap_read_once
Default: false.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
Contains classes for various device interfaces.
void * m_pcap_bpf_program
opaque ptr: bpf_program*
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
STL namespace.
std::shared_ptr< mrpt::obs ::CObservationGPS > Ptr
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores (&#39;_&#39;) or any other user-given char. ...
Definition: filesystem.cpp:329
GLdouble s
Definition: glext.h:3682
void reverseBytesInPlace(bool &v_in_out)
Reverse the order of the bytes of a given type (useful for transforming btw little/big endian) ...
int8_t validity_char
This will be: &#39;A&#39;=OK or &#39;V&#39;=void.
bool m_pcap_read_fast
(Default: false) If false, will use m_pcap_read_full_scan_delay_ms
UTC_time UTCTime
The GPS sensor measured timestamp (in UTC time)
mrpt::system::TTimeStamp m_last_pos_packet_timestamp
char NMEA_GPRMC[72+234]
the full $GPRMC message, as received by Velodyne, terminated with "\r\n\0"
ENUMTYPE read_enum(const std::string &section, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
double m_pcap_repeat_delay
Default: 0 (in seconds)
mrpt::obs::VelodyneCalibration m_velodyne_calib
Device calibration file (supplied by vendor in an XML file)
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
This class allows loading and storing values and vectors of different types from a configuration text...
ERRORCODE_HTTP
Possible returns from a HTTP request.
Definition: net_utils.h:31
double m_pos_packets_min_period
Default: 0.5 seconds.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &section) override
See the class documentation at the top for expected parameters.
bool setLidarOnOff(bool on)
Switches the LASER on/off (saves energy when not measuring) (via HTTP post interface).
A helper class that can convert an enum value into its textual representation, and viceversa...
constexpr double DEG2RAD(const double x)
Degrees to radians.
nv_oem6_header_t header
Novatel frame: NV_OEM6_BESTPOS.
bool receivePackets(mrpt::system::TTimeStamp &data_pkt_timestamp, mrpt::obs::CObservationVelodyneScan::TVelodyneRawPacket &out_data_pkt, mrpt::system::TTimeStamp &pos_pkt_timestamp, mrpt::obs::CObservationVelodyneScan::TVelodynePositionPacket &out_pos_pkt)
Users normally would prefer calling getNextObservation() instead.
The parts of a date/time (it&#39;s like the standard &#39;tm&#39; but with fractions of seconds).
Definition: datetime.h:49
unsigned int m_pcap_read_count
number of pkts read from the file so far (for debugging)
std::string m_pcap_output_file
Default: "" (do not dump to an offline file)
uint8_t day
Month (1-12)
Definition: datetime.h:53
static const model_properties_list_t & get()
Singleton access.
This namespace contains representation of robot actions and observations.
bool m_pcap_verbose
Default: true Output PCAP Info msgs.
constexpr auto sect
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
bool setLidarReturnType(return_type_t ret_type)
Changes among STRONGEST, LAST, DUAL return types (via HTTP post interface).
GLsizei const GLchar ** string
Definition: glext.h:4116
uint16_t header
Block id: UPPER_BANK or LOWER_BANK.
std::string m_pcap_input_file
Default: "" (do not operate from an offline file)
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
static bool parse_NMEA(const std::string &cmd_line, mrpt::obs::CObservationGPS &out_obs, const bool verbose=false)
Parses one line of NMEA data from a GPS receiver, and writes the recognized fields (if any) into an o...
void setFramePublishing(bool on)
Switches whole frame (points in a single revolution) on/off publication to data packet publication...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:410
void initialize() override
Tries to initialize the sensor driver, after setting all the parameters with a call to loadConfig...
double second
Minute (0-59)
Definition: datetime.h:56
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
void * m_pcap_out
opaque ptr: "pcap_t*"
bool internal_send_http_post(const std::string &post_data)
mrpt::system::TTimeStamp getDateAsTimestamp() const
Build an MRPT timestamp with the year/month/day of this observation.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double m_pos_packets_timing_timeout
Default: 30 seconds.
uint8_t minute
Hour (0-23)
Definition: datetime.h:55
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
One unit of data from the scanner (the payload of one UDP DATA packet)
bool empty() const
Returns true if no calibration has been loaded yet.
#define MRPT_END
Definition: exceptions.h:245
static short int VELODYNE_DATA_UDP_PORT
Default: 2368.
double m_pcap_read_full_scan_delay_ms
(Default:100 ms) delay after each full scan read from a PCAP log
void * m_pcap
opaque ptr: "pcap_t*"
uint8_t month
The year.
Definition: datetime.h:52
ERRORCODE_HTTP http_request(const string &http_method, const string &http_send_content, const string &url, std::vector< uint8_t > &out_content, string &out_errormsg, int port=80, const string &auth_user=string(), const string &auth_pass=string(), int *out_http_responsecode=nullptr, mrpt::system::TParameters< string > *extra_headers=nullptr, mrpt::system::TParameters< string > *out_headers=nullptr, int timeout_ms=1000)
Generic function for HTTP GET & POST methods.
Definition: net_utils.cpp:69
A set of useful routines for networking.
Definition: net_utils.h:23
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
uint8_t hour
Day (1-31)
Definition: datetime.h:54
uint32_t platform_socket_t
Handles for the UDP sockets, or INVALID_SOCKET (-1)
uint16_t rotation
0-35999, divide by 100 to get degrees
double 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.h:123
GLuint res
Definition: glext.h:7385
void * m_pcap_dumper
opaque ptr: "pcap_dumper_t *"
std::shared_ptr< mrpt::obs ::CObservationVelodyneScan > Ptr
void doProcess() override
This method will be invoked at a minimum rate of "process_rate" (Hz)
std::string m_device_ip
Default: "" (no IP-based filtering)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
bool loadCalibrationFile(const std::string &velodyne_xml_calib_file_path)
Returns false on error.
For usage when passing a dynamic number of (numeric) arguments to a function, by name.
Definition: TParameters.h:54
static Ptr Create(Args &&... args)
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
mrpt::system::TTimeStamp m_last_gps_rmc_age
mrpt::obs::CObservationVelodyneScan::Ptr m_rx_scan
In progress RX scan.
static mrpt::system::TTimeStamp internal_receive_UDP_packet(platform_socket_t hSocket, uint8_t *out_buffer, const size_t expected_packet_size, const std::string &filter_only_from_IP)
model_t m_model
Default: "VLP16".
mrpt::obs::gnss::Message_NMEA_RMC m_last_gps_rmc
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
bool setLidarRPM(int rpm)
Changes Lidar RPM (valid range: 300-600) (via HTTP post interface).



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: cb560b230 Wed Nov 13 08:06:48 2019 +0100 at miƩ nov 13 08:15:10 CET 2019