Main MRPT website > C++ reference for MRPT 1.9.9
CVelodyneScanner.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 
14 #include <mrpt/comms/net_utils.h>
16 #include <mrpt/system/filesystem.h>
18 #include <thread>
19 
20 // socket's hdrs:
21 #ifdef _WIN32
22 #define _WINSOCK_DEPRECATED_NO_WARNINGS
23 #if defined(_WIN32_WINNT) && (_WIN32_WINNT < 0x600)
24 #undef _WIN32_WINNT
25 #define _WIN32_WINNT 0x600 // Minimum: Windows Vista (required to pollfd)
26 #endif
27 
28 #include <winsock2.h>
29 using socklen_t = int;
30 
31 #if defined(_MSC_VER)
32 #pragma comment(lib, "WS2_32.LIB")
33 #endif
34 
35 #else
36 #define INVALID_SOCKET (-1)
37 #include <sys/time.h> // gettimeofday()
38 #include <sys/socket.h>
39 #include <unistd.h>
40 #include <fcntl.h>
41 #include <errno.h>
42 #include <sys/types.h>
43 #include <sys/ioctl.h>
44 #include <netdb.h>
45 #include <arpa/inet.h>
46 #include <netinet/in.h>
47 #include <netinet/tcp.h>
48 #include <poll.h>
49 #endif
50 
51 #if MRPT_HAS_LIBPCAP
52 #include <pcap.h>
53 #if !defined(PCAP_NETMASK_UNKNOWN) // for older pcap versions
54 #define PCAP_NETMASK_UNKNOWN 0xffffffff
55 #endif
56 #endif
57 
58 using namespace mrpt;
59 using namespace mrpt::hwdrivers;
60 using namespace mrpt::obs;
61 using namespace std;
62 
64 
67 
70 {
71  static CVelodyneScanner::model_properties_list_t modelProperties;
72  static bool init = false;
73  if (!init)
74  {
75  init = true;
76  modelProperties[CVelodyneScanner::VLP16].maxRange = 120.0;
77  modelProperties[CVelodyneScanner::HDL32].maxRange = 70.0;
78  modelProperties[CVelodyneScanner::HDL64].maxRange = 120.0;
79  }
80  return modelProperties;
81 }
83 {
86  std::string s;
88  lst.begin();
89  it != lst.end(); ++it)
90  s += mrpt::format(
91  "`%s`,",
93  it->first)
94  .c_str());
95  return s;
96 }
97 
99  : m_model(CVelodyneScanner::VLP16),
100  m_pos_packets_min_period(0.5),
101  m_pos_packets_timing_timeout(30.0),
102  m_device_ip(""),
103  m_return_frames(true),
104  m_pcap_verbose(true),
105  m_last_pos_packet_timestamp(INVALID_TIMESTAMP),
106  m_pcap(nullptr),
107  m_pcap_out(nullptr),
108  m_pcap_dumper(nullptr),
109  m_pcap_bpf_program(nullptr),
110  m_pcap_file_empty(true),
111  m_pcap_read_once(false),
112  m_pcap_read_fast(false),
113  m_pcap_read_full_scan_delay_ms(100),
114  m_pcap_repeat_delay(0.0),
115  m_hDataSock(INVALID_SOCKET),
116  m_hPositionSock(INVALID_SOCKET),
117  m_last_gps_rmc_age(INVALID_TIMESTAMP),
118  m_lidar_rpm(0),
119  m_lidar_return(UNCHANGED)
120 {
121  m_sensorLabel = "Velodyne";
122 
123 #if MRPT_HAS_LIBPCAP
124  m_pcap_bpf_program = new bpf_program[1];
125 #endif
126 
127 #ifdef _WIN32
128  // Init the WinSock Library:
129  WORD wVersionRequested = MAKEWORD(2, 0);
130  WSADATA wsaData;
131  if (WSAStartup(wVersionRequested, &wsaData))
132  THROW_EXCEPTION("Error calling WSAStartup");
133 #endif
134 }
135 
137 {
138  this->close();
139 #ifdef _WIN32
140  WSACleanup();
141 #endif
142 
143 #if MRPT_HAS_LIBPCAP
144  delete[] reinterpret_cast<bpf_program*>(m_pcap_bpf_program);
145  m_pcap_bpf_program = nullptr;
146 #endif
147 }
148 
150  const std::string& velodyne_xml_calib_file_path)
151 {
152  return m_velodyne_calib.loadFromXMLFile(velodyne_xml_calib_file_path);
153 }
154 
156  const mrpt::config::CConfigFileBase& cfg, const std::string& sect)
157 {
158  MRPT_START
159 
160  cfg.read_enum<CVelodyneScanner::model_t>(sect, "model", m_model);
161  MRPT_LOAD_HERE_CONFIG_VAR(device_ip, string, m_device_ip, cfg, sect);
162  MRPT_LOAD_HERE_CONFIG_VAR(pcap_input, string, m_pcap_input_file, cfg, sect);
164  pcap_output, string, m_pcap_output_file, cfg, sect);
166  pcap_read_once, bool, m_pcap_read_once, cfg, sect);
168  pcap_read_fast, bool, m_pcap_read_fast, cfg, sect);
170  pcap_read_full_scan_delay_ms, double, m_pcap_read_full_scan_delay_ms,
171  cfg, sect);
173  pcap_repeat_delay, double, m_pcap_repeat_delay, cfg, sect);
175  pos_packets_timing_timeout, double, m_pos_packets_timing_timeout, cfg,
176  sect);
178  pos_packets_min_period, double, m_pos_packets_min_period, cfg, sect);
179 
180  using mrpt::DEG2RAD;
182  cfg.read_float(sect, "pose_x", 0), cfg.read_float(sect, "pose_y", 0),
183  cfg.read_float(sect, "pose_z", 0),
184  DEG2RAD(cfg.read_float(sect, "pose_yaw", 0)),
185  DEG2RAD(cfg.read_float(sect, "pose_pitch", 0)),
186  DEG2RAD(cfg.read_float(sect, "pose_roll", 0)));
187 
188  std::string calibration_file;
189  MRPT_LOAD_CONFIG_VAR(calibration_file, string, cfg, sect);
190  if (!calibration_file.empty()) this->loadCalibrationFile(calibration_file);
191 
192  // Check validity:
194  if (lstModels.find(m_model) == lstModels.end())
195  {
197  mrpt::format(
198  "Unrecognized `model` parameter: `%u` . Known values are: %s",
199  static_cast<unsigned int>(m_model),
201  }
202 
203  // Optional HTTP-based settings:
204  MRPT_LOAD_HERE_CONFIG_VAR(rpm, int, m_lidar_rpm, cfg, sect);
206  cfg.read_enum<return_type_t>(sect, "return_type", m_lidar_return);
207 
208  MRPT_END
209 }
210 
214 {
215  try
216  {
217  ASSERTMSG_(m_initialized, "initialize() has not been called yet!");
218 
219  // Init with empty smart pointers:
222 
223  // Try to get data & pos packets:
226  mrpt::system::TTimeStamp data_pkt_timestamp, pos_pkt_timestamp;
227  bool rx_all_ok = this->receivePackets(
228  data_pkt_timestamp, rx_pkt, pos_pkt_timestamp, rx_pos_pkt);
229 
230  if (!rx_all_ok)
231  {
232  // PCAP EOF:
233  return false;
234  }
235 
236  if (pos_pkt_timestamp != INVALID_TIMESTAMP)
237  {
239  mrpt::make_aligned_shared<mrpt::obs::CObservationGPS>();
240  gps_obs->sensorLabel = this->m_sensorLabel + std::string("_GPS");
241  gps_obs->sensorPose = m_sensorPose;
242 
243  gps_obs->originalReceivedTimestamp = pos_pkt_timestamp;
244 
245  bool parsed_ok = CGPSInterface::parse_NMEA(
246  std::string(rx_pos_pkt.NMEA_GPRMC), *gps_obs);
247  const mrpt::obs::gnss::Message_NMEA_RMC* msg_rmc =
248  gps_obs->getMsgByClassPtr<mrpt::obs::gnss::Message_NMEA_RMC>();
249  if (!parsed_ok || !msg_rmc || msg_rmc->fields.validity_char != 'A')
250  {
251  gps_obs->has_satellite_timestamp = false;
252  gps_obs->timestamp = pos_pkt_timestamp;
253  }
254  else
255  {
256  // We have live GPS signal and a recent RMC frame:
257  m_last_gps_rmc_age = pos_pkt_timestamp;
258  m_last_gps_rmc = *msg_rmc;
259  }
260  outGPS = gps_obs; // save in output object
261  }
262 
263  if (data_pkt_timestamp != INVALID_TIMESTAMP)
264  {
265  m_state = ssWorking;
266 
267  // Break into a new observation object when the azimuth passes
268  // 360->0 deg:
269  const uint16_t rx_pkt_start_angle = rx_pkt.blocks[0].rotation;
270  // const uint16_t rx_pkt_end_angle =
271  // rx_pkt.blocks[CObservationVelodyneScan::BLOCKS_PER_PACKET-1].rotation;
272 
273  // Return the observation as done when a complete 360 deg scan is
274  // ready:
275  if (m_rx_scan && !m_rx_scan->scan_packets.empty())
276  {
277  if ((rx_pkt_start_angle <
278  m_rx_scan->scan_packets.rbegin()->blocks[0].rotation) ||
280  {
281  outScan = m_rx_scan;
282  m_rx_scan.reset();
283 
284  if (m_pcap)
285  {
286  // Keep the reader from blowing through the file.
287  if (!m_pcap_read_fast)
288  std::this_thread::sleep_for(
289  std::chrono::duration<double, std::milli>(
291  }
292  }
293  }
294 
295  // Create smart ptr to new in-progress observation:
296  if (!m_rx_scan)
297  {
300  m_rx_scan->sensorLabel =
301  this->m_sensorLabel + std::string("_SCAN");
302  m_rx_scan->sensorPose = m_sensorPose;
303  m_rx_scan->calibration =
304  m_velodyne_calib; // Embed a copy of the calibration info
305 
306  {
307  const model_properties_list_t& lstModels =
310  lstModels.find(this->m_model);
311  if (it != lstModels.end())
312  { // Model params:
313  m_rx_scan->maxRange = it->second.maxRange;
314  }
315  else // default params:
316  {
317  m_rx_scan->maxRange = 120.0;
318  }
319  }
320  }
321 
322  // For the first packet, set timestamp:
323  if (m_rx_scan->scan_packets.empty())
324  {
325  m_rx_scan->originalReceivedTimestamp = data_pkt_timestamp;
326  // Using GPS, if available:
327  if (m_last_gps_rmc.fields.validity_char == 'A' &&
329  m_last_gps_rmc_age, data_pkt_timestamp) <
331  {
332  // Each Velodyne data packet has a timestamp field,
333  // with the number of us since the top of the current HOUR:
334  // take the date and time from the GPS, then modify minutes
335  // and seconds from data pkt:
336  const mrpt::system::TTimeStamp gps_tim =
339 
340  mrpt::system::TTimeParts tim_parts;
341  mrpt::system::timestampToParts(gps_tim, tim_parts);
342  tim_parts.minute =
343  rx_pkt.gps_timestamp /*us from top of hour*/ /
344  60000000ul;
345  tim_parts.second =
346  (rx_pkt.gps_timestamp /*us from top of hour*/ %
347  60000000ul) *
348  1e-6;
349 
350  const mrpt::system::TTimeStamp data_pkt_tim =
352 
353  m_rx_scan->timestamp = data_pkt_tim;
354  m_rx_scan->has_satellite_timestamp = true;
355  }
356  else
357  {
358  m_rx_scan->has_satellite_timestamp = false;
359  m_rx_scan->timestamp = data_pkt_timestamp;
360  }
361  }
362 
363  // Accumulate pkts in the observation object:
364  m_rx_scan->scan_packets.push_back(rx_pkt);
365  }
366 
367  return true;
368  }
369  catch (exception& e)
370  {
371  cerr << "[CVelodyneScanner::getObservation] Returning false due to "
372  "exception: "
373  << endl;
374  cerr << e.what() << endl;
375  return false;
376  }
377 }
378 
380 {
382  CObservationGPS::Ptr obs_gps;
383 
384  if (getNextObservation(obs, obs_gps))
385  {
386  m_state = ssWorking;
387  if (obs) appendObservation(obs);
388  if (obs_gps) appendObservation(obs_gps);
389  }
390  else
391  {
392  m_state = ssError;
393  cerr << "ERROR receiving data from Velodyne devic!" << endl;
394  }
395 }
396 
397 /** Tries to initialize the sensor, after setting all the parameters with a call
398 * to loadConfig.
399 * \exception This method must throw an exception with a descriptive message if
400 * some critical error is found. */
402 {
403  this->close();
404 
405  // (0) Preparation:
406  // --------------------------------
407  // Make sure we have calibration data:
408  if (m_velodyne_calib.empty())
409  {
410  // Try to load default data:
411  m_velodyne_calib = VelodyneCalibration::LoadDefaultCalibration(
413  m_model));
414  if (m_velodyne_calib.empty())
416  "Could not find default calibration data for the given LIDAR "
417  "`model` name. Please, specify a valid `model` or load a valid "
418  "XML configuration file first.");
419  }
420 
421  // online vs off line operation??
422  // -------------------------------
423  if (m_pcap_input_file.empty())
424  { // Online
425 
426  if (m_lidar_rpm > 0)
427  {
428  if (!setLidarRPM(m_lidar_rpm))
429  THROW_EXCEPTION("Error in setLidarRPM();");
430  }
431  if (m_lidar_return != UNCHANGED)
432  {
434  THROW_EXCEPTION("Error in setLidarReturnType();");
435  }
436 
437  // (1) Create LIDAR DATA socket
438  // --------------------------------
439  if (INVALID_SOCKET == (m_hDataSock = socket(PF_INET, SOCK_DGRAM, 0)))
441  format(
442  "Error creating UDP socket:\n%s",
444 
445  struct sockaddr_in bindAddr;
446  memset(&bindAddr, 0, sizeof(bindAddr));
447  bindAddr.sin_family = AF_INET;
448  bindAddr.sin_port = htons(VELODYNE_DATA_UDP_PORT);
449  bindAddr.sin_addr.s_addr = INADDR_ANY;
450 
451  if (int(INVALID_SOCKET) ==
452  ::bind(
453  m_hDataSock, (struct sockaddr*)(&bindAddr), sizeof(sockaddr)))
455 
456 #ifdef _WIN32
457  unsigned long non_block_mode = 1;
458  if (ioctlsocket(m_hDataSock, FIONBIO, &non_block_mode))
460  "Error entering non-blocking mode with ioctlsocket().");
461 #else
462  int oldflags = fcntl(m_hDataSock, F_GETFL, 0);
463  if (oldflags == -1)
464  THROW_EXCEPTION("Error retrieving fcntl() of socket.");
465  oldflags |= O_NONBLOCK | FASYNC;
466  if (-1 == fcntl(m_hDataSock, F_SETFL, oldflags))
467  THROW_EXCEPTION("Error entering non-blocking mode with fcntl();");
468 #endif
469 
470  // (2) Create LIDAR POSITION socket
471  // --------------------------------
472  if (INVALID_SOCKET ==
473  (m_hPositionSock = socket(PF_INET, SOCK_DGRAM, 0)))
475  format(
476  "Error creating UDP socket:\n%s",
478 
479  bindAddr.sin_port = htons(VELODYNE_POSITION_UDP_PORT);
480 
481  if (int(INVALID_SOCKET) == ::bind(
483  (struct sockaddr*)(&bindAddr),
484  sizeof(sockaddr)))
486 
487 #ifdef _WIN32
488  if (ioctlsocket(m_hPositionSock, FIONBIO, &non_block_mode))
490  "Error entering non-blocking mode with ioctlsocket().");
491 #else
492  oldflags = fcntl(m_hPositionSock, F_GETFL, 0);
493  if (oldflags == -1)
494  THROW_EXCEPTION("Error retrieving fcntl() of socket.");
495  oldflags |= O_NONBLOCK | FASYNC;
496  if (-1 == fcntl(m_hPositionSock, F_SETFL, oldflags))
497  THROW_EXCEPTION("Error entering non-blocking mode with fcntl();");
498 #endif
499  }
500  else
501  { // Offline:
502 #if MRPT_HAS_LIBPCAP
503  char errbuf[PCAP_ERRBUF_SIZE];
504 
505  if (m_pcap_verbose)
506  printf(
507  "\n[CVelodyneScanner] Opening PCAP file \"%s\"\n",
508  m_pcap_input_file.c_str());
509  if ((m_pcap = pcap_open_offline(m_pcap_input_file.c_str(), errbuf)) ==
510  nullptr)
511  {
512  THROW_EXCEPTION_FMT("Error opening PCAP file: '%s'", errbuf);
513  }
514 
515  // Build PCAP filter:
516  {
517  std::string filter_str = mrpt::format(
518  "(udp dst port %d || udp dst port %d)",
521  if (!m_device_ip.empty())
522  filter_str += "&& src host " + m_device_ip;
523 
524  static std::string sMsgError =
525  "[CVelodyneScanner] Error calling pcap_compile: "; // This is
526  // to avoid
527  // the
528  // ill-formed
529  // signature
530  // of
531  // pcap_error()
532  // accepting
533  // "char*",
534  // not
535  // "const
536  // char*"...
537  // sigh
538  if (pcap_compile(
539  reinterpret_cast<pcap_t*>(m_pcap),
540  reinterpret_cast<bpf_program*>(m_pcap_bpf_program),
541  filter_str.c_str(), 1, PCAP_NETMASK_UNKNOWN) < 0)
542  pcap_perror(reinterpret_cast<pcap_t*>(m_pcap), &sMsgError[0]);
543  }
544 
545  m_pcap_file_empty = true;
546  m_pcap_read_count = 0;
547 
548 #else
550  "Velodyne: Reading from PCAP requires building MRPT with libpcap "
551  "support!");
552 #endif
553  }
554 
558 
559  m_initialized = true;
560 }
561 
563 {
564  if (m_hDataSock != INVALID_SOCKET)
565  {
566  shutdown(m_hDataSock, 2); // SD_BOTH );
567 #ifdef _WIN32
568  closesocket(m_hDataSock);
569 #else
571 #endif
572  m_hDataSock = INVALID_SOCKET;
573  }
574 
575  if (m_hPositionSock != INVALID_SOCKET)
576  {
577  shutdown(m_hPositionSock, 2); // SD_BOTH );
578 #ifdef _WIN32
579  closesocket(m_hPositionSock);
580 #else
582 #endif
583  m_hPositionSock = INVALID_SOCKET;
584  }
585 #if MRPT_HAS_LIBPCAP
586  if (m_pcap)
587  {
588  pcap_close(reinterpret_cast<pcap_t*>(m_pcap));
589  m_pcap = nullptr;
590  }
591  if (m_pcap_dumper)
592  {
593  pcap_dump_close(reinterpret_cast<pcap_dumper_t*>(m_pcap_dumper));
594  m_pcap_dumper = nullptr;
595  }
596  if (m_pcap_out)
597  {
598  pcap_close(reinterpret_cast<pcap_t*>(m_pcap_out));
599  m_pcap_out = nullptr;
600  }
601 #endif
602  m_initialized = false;
603 }
604 
605 // Fixed Ethernet headers for PCAP capture --------
606 #if MRPT_HAS_LIBPCAP
607 const uint16_t LidarPacketHeader[21] = {0xffff, 0xffff, 0xffff, 0x7660, 0x0088,
608  0x0000, 0x0008, 0x0045, 0xd204, 0x0000,
609  0x0040, 0x11ff, 0xaab4, 0xa8c0, 0xc801,
610  0xffff, // checksum 0xa9b4 //source ip
611  // 0xa8c0, 0xc801 is
612  // 192.168.1.200
613  0xffff, 0x4009, 0x4009, 0xbe04, 0x0000};
614 const uint16_t PositionPacketHeader[21] = {
615  0xffff, 0xffff, 0xffff, 0x7660, 0x0088, 0x0000, 0x0008, 0x0045, 0xd204,
616  0x0000, 0x0040, 0x11ff, 0xaab4, 0xa8c0, 0xc801, 0xffff, // checksum 0xa9b4
617  // //source ip
618  // 0xa8c0, 0xc801
619  // is 192.168.1.200
620  0xffff, 0x7420, 0x7420, 0x0802, 0x0000};
621 #endif
622 
623 #if defined(_WIN32) && MRPT_HAS_LIBPCAP
624 int gettimeofday(struct timeval* tp, void*)
625 {
626  FILETIME ft;
627  ::GetSystemTimeAsFileTime(&ft);
628  long long t =
629  (static_cast<long long>(ft.dwHighDateTime) << 32) | ft.dwLowDateTime;
630  t -= 116444736000000000LL;
631  t /= 10; // microseconds
632  tp->tv_sec = static_cast<long>(t / 1000000UL);
633  tp->tv_usec = static_cast<long>(t % 1000000UL);
634  return 0;
635 }
636 #endif
637 
639  mrpt::system::TTimeStamp& data_pkt_timestamp,
641  mrpt::system::TTimeStamp& pos_pkt_timestamp,
643 {
646  CObservationVelodyneScan::POS_PACKET_SIZE);
649  CObservationVelodyneScan::PACKET_SIZE);
650 
651  bool ret = true; // all ok
652  if (m_pcap)
653  {
655  data_pkt_timestamp, (uint8_t*)&out_data_pkt, pos_pkt_timestamp,
656  (uint8_t*)&out_pos_pkt);
657  }
658  else
659  {
660  data_pkt_timestamp = internal_receive_UDP_packet(
661  m_hDataSock, (uint8_t*)&out_data_pkt,
662  CObservationVelodyneScan::PACKET_SIZE, m_device_ip);
663  pos_pkt_timestamp = internal_receive_UDP_packet(
664  m_hPositionSock, (uint8_t*)&out_pos_pkt,
665  CObservationVelodyneScan::POS_PACKET_SIZE, m_device_ip);
666  }
667 
668 // Optional PCAP dump:
669 #if MRPT_HAS_LIBPCAP
670  // Save to PCAP file?
671  if (!m_pcap_output_file.empty() &&
672  !m_pcap_out) // 1st time: create output file
673  {
674 #if MRPT_HAS_LIBPCAP
675  char errbuf[PCAP_ERRBUF_SIZE];
676 
679  string sFilePostfix = "_";
680  sFilePostfix += format(
681  "%04u-%02u-%02u_%02uh%02um%02us", (unsigned int)parts.year,
682  (unsigned int)parts.month, (unsigned int)parts.day,
683  (unsigned int)parts.hour, (unsigned int)parts.minute,
684  (unsigned int)parts.second);
685  const string sFileName =
688  string(".pcap");
689 
690  m_pcap_out = pcap_open_dead(DLT_EN10MB, 65535);
691  ASSERTMSG_(
692  m_pcap_out != nullptr, "Error creating PCAP live capture handle");
693 
694  printf(
695  "\n[CVelodyneScanner] Writing to PCAP file \"%s\"\n",
696  sFileName.c_str());
697  if ((m_pcap_dumper = pcap_dump_open(
698  reinterpret_cast<pcap_t*>(m_pcap_out), sFileName.c_str())) ==
699  nullptr)
700  {
702  "Error creating PCAP live dumper: '%s'", errbuf);
703  }
704 #else
706  "Velodyne: Writing PCAP files requires building MRPT with libpcap "
707  "support!");
708 #endif
709  }
710 
711  if (m_pcap_out && m_pcap_dumper &&
712  (data_pkt_timestamp != INVALID_TIMESTAMP ||
713  pos_pkt_timestamp != INVALID_TIMESTAMP))
714  {
715  struct pcap_pkthdr header;
716  struct timeval currentTime;
717  gettimeofday(&currentTime, nullptr);
718  std::vector<unsigned char> packetBuffer;
719 
720  // Data pkt:
721  if (data_pkt_timestamp != INVALID_TIMESTAMP)
722  {
723  header.caplen = header.len =
724  CObservationVelodyneScan::PACKET_SIZE + 42;
725  packetBuffer.resize(header.caplen);
726  header.ts = currentTime;
727 
728  memcpy(&(packetBuffer[0]), LidarPacketHeader, 42);
729  memcpy(
730  &(packetBuffer[0]) + 42, (uint8_t*)&out_data_pkt,
731  CObservationVelodyneScan::PACKET_SIZE);
732  pcap_dump(
733  (u_char*)this->m_pcap_dumper, &header, &(packetBuffer[0]));
734  }
735  // Pos pkt:
736  if (pos_pkt_timestamp != INVALID_TIMESTAMP)
737  {
738  header.caplen = header.len =
739  CObservationVelodyneScan::POS_PACKET_SIZE + 42;
740  packetBuffer.resize(header.caplen);
741  header.ts = currentTime;
742 
743  memcpy(&(packetBuffer[0]), PositionPacketHeader, 42);
744  memcpy(
745  &(packetBuffer[0]) + 42, (uint8_t*)&out_pos_pkt,
746  CObservationVelodyneScan::POS_PACKET_SIZE);
747  pcap_dump(
748  (u_char*)this->m_pcap_dumper, &header, &(packetBuffer[0]));
749  }
750  }
751 #endif
752 
753 // Convert from Velodyne's standard little-endian ordering to host byte
754 // ordering:
755 // (done AFTER saving the pckg as is to pcap above)
756 #if MRPT_IS_BIG_ENDIAN
757  if (data_pkt_timestamp != INVALID_TIMESTAMP)
758  {
760  for (int i = 0; i < CObservationVelodyneScan::BLOCKS_PER_PACKET; i++)
761  {
764  for (int k = 0; k < CObservationVelodyneScan::SCANS_PER_BLOCK; k++)
765  {
767  out_data_pkt.blocks[i].laser_returns[k].distance);
768  }
769  }
770  }
771  if (pos_pkt_timestamp != INVALID_TIMESTAMP)
772  {
775  }
776 #endif
777 
778  // Position packet decimation:
779  if (pos_pkt_timestamp != INVALID_TIMESTAMP)
780  {
783  m_last_pos_packet_timestamp, pos_pkt_timestamp) <
785  {
786  // Ignore this packet
787  pos_pkt_timestamp = INVALID_TIMESTAMP;
788  }
789  else
790  {
791  // Reset time watch:
792  m_last_pos_packet_timestamp = pos_pkt_timestamp;
793  }
794  }
795 
796  return ret;
797 }
798 
799 // static method:
801  platform_socket_t hSocket, uint8_t* out_buffer,
802  const size_t expected_packet_size, const std::string& filter_only_from_IP)
803 {
804  if (hSocket == INVALID_SOCKET)
806  "Error: UDP socket is not open yet! Have you called initialize() "
807  "first?");
808 
809  unsigned long devip_addr = 0;
810  if (!filter_only_from_IP.empty())
811  devip_addr = inet_addr(filter_only_from_IP.c_str());
812 
814 
815  struct pollfd fds[1];
816  fds[0].fd = hSocket;
817  fds[0].events = POLLIN;
818  static const int POLL_TIMEOUT = 1; // (ms)
819 
820  sockaddr_in sender_address;
821  socklen_t sender_address_len = sizeof(sender_address);
822 
823  while (true)
824  {
825  // Unfortunately, the Linux kernel recvfrom() implementation
826  // uses a non-interruptible std::this_thread::sleep_for(ms) when waiting
827  // for data,
828  // which would cause this method to hang if the device is not
829  // providing data. We poll() the device first to make sure
830  // the recvfrom() will not block.
831  //
832  // Note, however, that there is a known Linux kernel bug:
833  //
834  // Under Linux, select() may report a socket file descriptor
835  // as "ready for reading", while nevertheless a subsequent
836  // read blocks. This could for example happen when data has
837  // arrived but upon examination has wrong checksum and is
838  // discarded. There may be other circumstances in which a
839  // file descriptor is spuriously reported as ready. Thus it
840  // may be safer to use O_NONBLOCK on sockets that should not
841  // block.
842 
843  // poll() until input available
844  do
845  {
846  int retval =
847 #if !defined(_WIN32)
848  poll
849 #else
850  WSAPoll
851 #endif
852  (fds, 1, POLL_TIMEOUT);
853  if (retval < 0) // poll() error?
854  {
855  if (errno != EINTR)
857  format(
858  "Error in UDP poll():\n%s",
860  }
861  if (retval == 0) // poll() timeout?
862  {
863  return INVALID_TIMESTAMP;
864  }
865  if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) ||
866  (fds[0].revents & POLLNVAL)) // device error?
867  {
869  "Error in UDP poll() (seems Velodyne device error?)");
870  }
871  } while ((fds[0].revents & POLLIN) == 0);
872 
873  // Receive packets that should now be available from the
874  // socket using a blocking read.
875  int nbytes = recvfrom(
876  hSocket, (char*)&out_buffer[0], expected_packet_size, 0,
877  (sockaddr*)&sender_address, &sender_address_len);
878 
879  if (nbytes < 0)
880  {
881  if (errno != EWOULDBLOCK) THROW_EXCEPTION("recvfrom() failed!?!");
882  }
883  else if ((size_t)nbytes == expected_packet_size)
884  {
885  // read successful,
886  // if packet is not from the lidar scanner we selected by IP,
887  // continue otherwise we are done
888  if (!filter_only_from_IP.empty() &&
889  sender_address.sin_addr.s_addr != devip_addr)
890  continue;
891  else
892  break; // done
893  }
894 
895  std::cerr
896  << "[CVelodyneScanner] Warning: incomplete Velodyne packet read: "
897  << nbytes << " bytes\n";
898  }
899 
900  // Average the times at which we begin and end reading. Use that to
901  // estimate when the scan occurred.
903 
904  return (time1 / 2 + time2 / 2);
905 }
906 
908  mrpt::system::TTimeStamp& data_pkt_time, uint8_t* out_data_buffer,
909  mrpt::system::TTimeStamp& pos_pkt_time, uint8_t* out_pos_buffer)
910 {
911 #if MRPT_HAS_LIBPCAP
912  ASSERT_(m_pcap);
913 
914  data_pkt_time = INVALID_TIMESTAMP;
915  pos_pkt_time = INVALID_TIMESTAMP;
916 
917  char errbuf[PCAP_ERRBUF_SIZE];
918  struct pcap_pkthdr* header;
919  const u_char* pkt_data;
920 
921  while (true)
922  {
923  int res;
924  if ((res = pcap_next_ex(
925  reinterpret_cast<pcap_t*>(m_pcap), &header, &pkt_data)) >= 0)
926  {
928 
929  // if packet is not from the lidar scanner we selected by IP,
930  // continue
931  if (pcap_offline_filter(
932  reinterpret_cast<bpf_program*>(m_pcap_bpf_program), header,
933  pkt_data) == 0)
934  {
935  if (m_verbose)
936  std::cout
937  << "[CVelodyneScanner] DEBUG: Filtering out packet #"
938  << m_pcap_read_count << " in PCAP file.\n";
939  continue;
940  }
941 
942  // Determine whether it is a DATA or POSITION packet:
943  m_pcap_file_empty = false;
945  const uint16_t udp_dst_port =
946  ntohs(*reinterpret_cast<const uint16_t*>(pkt_data + 0x24));
947 
949  {
950  if (m_verbose)
951  std::cout << "[CVelodyneScanner] DEBUG: Packet #"
953  << " in PCAP file is POSITION pkt.\n";
954  memcpy(
955  out_pos_buffer, pkt_data + 42,
956  CObservationVelodyneScan::POS_PACKET_SIZE);
957  pos_pkt_time = tim; // success
958  return true;
959  }
960  else if (udp_dst_port == CVelodyneScanner::VELODYNE_DATA_UDP_PORT)
961  {
962  if (m_verbose)
963  std::cout << "[CVelodyneScanner] DEBUG: Packet #"
965  << " in PCAP file is DATA pkt.\n";
966  memcpy(
967  out_data_buffer, pkt_data + 42,
968  CObservationVelodyneScan::PACKET_SIZE);
969  data_pkt_time = tim; // success
970  return true;
971  }
972  else
973  {
974  std::cerr << "[CVelodyneScanner] ERROR: Packet "
976  << " in PCAP file passed the filter but does not "
977  "match expected UDP port numbers! Skipping it.\n";
978  }
979  }
980 
981  if (m_pcap_file_empty) // no data in file?
982  {
983  fprintf(
984  stderr,
985  "[CVelodyneScanner] Maybe the PCAP file is empty? Error %d "
986  "reading Velodyne packet: `%s`\n",
987  res, pcap_geterr(reinterpret_cast<pcap_t*>(m_pcap)));
988  return true;
989  }
990 
991  if (m_pcap_read_once)
992  {
993  if (m_pcap_verbose)
994  printf(
995  "[CVelodyneScanner] INFO: end of file reached -- done "
996  "reading.\n");
997  std::this_thread::sleep_for(250ms);
998  return false;
999  }
1000 
1001  if (m_pcap_repeat_delay > 0.0)
1002  {
1003  if (m_pcap_verbose)
1004  printf(
1005  "[CVelodyneScanner] INFO: end of file reached -- delaying "
1006  "%.3f seconds.\n",
1008  std::this_thread::sleep_for(
1009  std::chrono::duration<double, std::milli>(
1010  m_pcap_repeat_delay * 1000.0));
1011  }
1012 
1013  if (m_pcap_verbose)
1014  printf("[CVelodyneScanner] INFO: replaying Velodyne dump file.\n");
1015 
1016  // rewind the file
1017  pcap_close(reinterpret_cast<pcap_t*>(m_pcap));
1018  if ((m_pcap = pcap_open_offline(m_pcap_input_file.c_str(), errbuf)) ==
1019  nullptr)
1020  {
1021  THROW_EXCEPTION_FMT("Error opening PCAP file: '%s'", errbuf);
1022  }
1023  m_pcap_file_empty = true; // maybe the file disappeared?
1024  } // loop back and try again
1025 #else
1027  "MRPT needs to be built against libpcap to enable this functionality")
1028 #endif
1029 }
1030 
1032 {
1033  /* HTTP-based config: http://10.0.0.100/tab/config.html
1034  <form name='returns' method="post" action="/cgi/setting">
1035  <span>Return&nbsp;Type:&nbsp;</span> <select name="returns"
1036  onchange='javascript:this.form.submit()'>
1037  <option>Strongest</option>
1038  <option>Last</option>
1039  <option>Dual</option>
1040  </select>
1041  </form>
1042  */
1043  MRPT_START;
1044  std::string strRet;
1045  switch (ret_type)
1046  {
1047  case STRONGEST:
1048  strRet = "Strongest";
1049  break;
1050  case DUAL:
1051  strRet = "Dual";
1052  break;
1053  case LAST:
1054  strRet = "Last";
1055  break;
1056  case UNCHANGED:
1057  return true;
1058  default:
1059  THROW_EXCEPTION("Invalid value for return type!");
1060  };
1061 
1062  const std::string cmd = mrpt::format("returns=%s", strRet.c_str());
1063  return this->internal_send_http_post(cmd);
1064  MRPT_END;
1065 }
1066 
1068 {
1069  /* HTTP-based config: http://10.0.0.100/tab/config.html
1070  <form name='rpm' method="post" action="/cgi/setting">
1071  Motor
1072  &nbsp;RPM:&nbsp;<input type="text" name="rpm" size="5"
1073  style="text-align:right" /><input type="button" value="+"
1074  onclick="javascript:this.form.rpm.value++;this.form.submit()" /><input
1075  type="button" value="-"
1076  onclick="javascript:this.form.rpm.value--;this.form.submit()" />
1077  &nbsp;<input type="submit" value="Set" />
1078  </form>
1079  */
1080 
1081  MRPT_START;
1082  const std::string cmd = mrpt::format("rpm=%i", rpm);
1083  return this->internal_send_http_post(cmd);
1084  MRPT_END;
1085 }
1086 
1088 {
1089  // laser = on|off
1090  MRPT_START;
1091  const std::string cmd = mrpt::format("laser=%s", on ? "on" : "off");
1092  return this->internal_send_http_post(cmd);
1093  MRPT_END;
1094 }
1095 
1097 {
1098  //frame publishing | data packet publishing = on|off
1099  MRPT_START;
1100  m_return_frames = on;
1101  MRPT_END;
1102 }
1103 
1105 {
1106  MRPT_START;
1107 
1108  ASSERTMSG_(
1109  !m_device_ip.empty(), "A device IP address must be specified first!");
1110 
1111  using namespace mrpt::comms::net;
1112 
1113  std::vector<uint8_t> post_out;
1114  string post_err_str;
1115 
1116  int http_rep_code;
1117  mrpt::system::TParameters<string> extra_headers, out_headers;
1118 
1119  extra_headers["Origin"] = mrpt::format("http://%s", m_device_ip.c_str());
1120  extra_headers["Referer"] = mrpt::format("http://%s", m_device_ip.c_str());
1121  extra_headers["Upgrade-Insecure-Requests"] = "1";
1122  extra_headers["Content-Type"] = "application/x-www-form-urlencoded";
1123 
1125  "POST", post_data,
1126  mrpt::format("http://%s/cgi/setting", m_device_ip.c_str()), post_out,
1127  post_err_str, 80 /* port */, string(), string(), // user,pass
1128  &http_rep_code, &extra_headers, &out_headers);
1129 
1130  return mrpt::comms::net::erOk == ret &&
1131  (http_rep_code == 200 || http_rep_code == 204); // OK codes
1132 
1133  MRPT_END;
1134 }
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:104
double time1
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:262
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
GLdouble GLdouble t
Definition: glext.h:3689
unsigned __int16 uint16_t
Definition: rptypes.h:44
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
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
std::string m_sensorLabel
See CGenericSensor.
double DEG2RAD(const double x)
Degrees to radians.
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:128
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:75
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::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:328
GLdouble s
Definition: glext.h:3676
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"
unsigned char uint8_t
Definition: rptypes.h:41
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:113
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
virtual void initialize()
Tries to initialize the sensor driver, after setting all the parameters with a call to loadConfig...
double m_pos_packets_min_period
Default: 0.5 seconds.
std::shared_ptr< CObservationVelodyneScan > Ptr
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...
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:37
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:41
static const model_properties_list_t & get()
Singleton access.
This namespace contains representation of robot actions and observations.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
bool m_pcap_verbose
Default: true Output PCAP Info msgs.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
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:4101
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...
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:15
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:406
double time2
double second
Minute (0-59)
Definition: datetime.h:44
#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:43
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
One unit of data from the scanner (the payload of one UDP DATA packet)
void reverseBytesInPlace(bool &v_in_out)
Reverse the order of the bytes of a given type (useful for transforming btw little/big endian) ...
bool empty() const
Returns true if no calibration has been loaded yet.
#define MRPT_END
Definition: exceptions.h:266
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*"
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
std::shared_ptr< CObservationGPS > Ptr
uint8_t month
The year.
Definition: datetime.h:40
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
void doProcess()
This method will be invoked at a minimum rate of "process_rate" (Hz)
A set of useful routines for networking.
Definition: net_utils.h:23
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
std::shared_ptr< T > make_aligned_shared(Args &&... args)
Creates a shared_ptr with aligned memory via aligned_allocator_cpp11<>.
uint8_t hour
Day (1-31)
Definition: datetime.h:42
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
#define MRPT_COMPILE_TIME_ASSERT(expression)
Definition: exceptions.h:127
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.cpp:209
GLuint res
Definition: glext.h:7268
void * m_pcap_dumper
opaque ptr: "pcap_dumper_t *"
std::string m_device_ip
Default: "" (no IP-based filtering)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
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:56
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &section)
See the class documentation at the top for expected parameters.
const Scalar * const_iterator
Definition: eigen_plugins.h:27
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:356
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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019