Main MRPT website > C++ reference for MRPT 1.5.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-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
13 #include <mrpt/utils/CStream.h>
14 #include <mrpt/utils/net_utils.h>
16 #include <mrpt/system/filesystem.h>
17 #include <mrpt/utils/bits.h> // for reverseBytesInPlace()
18 
19 // socket's hdrs:
20 #ifdef MRPT_OS_WINDOWS
21  #define _WINSOCK_DEPRECATED_NO_WARNINGS
22  #if defined(_WIN32_WINNT) && (_WIN32_WINNT<0x600)
23  #undef _WIN32_WINNT
24  #define _WIN32_WINNT 0x600 // Minimum: Windows Vista (required to pollfd)
25  #endif
26 
27  #include <winsock2.h>
28  typedef int socklen_t;
29 
30 # if defined(__BORLANDC__) || defined(_MSC_VER)
31 # pragma comment (lib,"WS2_32.LIB")
32 # endif
33 
34 #else
35  #define INVALID_SOCKET (-1)
36  #include <sys/time.h> // gettimeofday()
37  #include <sys/socket.h>
38  #include <unistd.h>
39  #include <fcntl.h>
40  #include <errno.h>
41  #include <sys/types.h>
42  #include <sys/ioctl.h>
43  #include <netdb.h>
44  #include <arpa/inet.h>
45  #include <netinet/in.h>
46  #include <netinet/tcp.h>
47  #include <poll.h>
48 #endif
49 
50 #if MRPT_HAS_LIBPCAP
51 # include <pcap.h>
52 # if !defined(PCAP_NETMASK_UNKNOWN) // for older pcap versions
53 # define PCAP_NETMASK_UNKNOWN 0xffffffff
54 # endif
55 #endif
56 
57 using namespace mrpt;
58 using namespace mrpt::hwdrivers;
59 using namespace mrpt::obs;
60 using namespace std;
61 
63 
66 
68 {
69  static CVelodyneScanner::model_properties_list_t modelProperties;
70  static bool init = false;
71  if (!init) {
72  init = true;
73  modelProperties[CVelodyneScanner::VLP16].maxRange = 120.0;
74  modelProperties[CVelodyneScanner::HDL32].maxRange = 70.0;
75  modelProperties[CVelodyneScanner::HDL64].maxRange = 120.0;
76  }
77  return modelProperties;
78 }
80 {
82  std::string s;
83  for (CVelodyneScanner::model_properties_list_t::const_iterator it=lst.begin();it!=lst.end();++it)
85  return s;
86 }
87 
88 
90  m_model(CVelodyneScanner::VLP16),
91  m_pos_packets_min_period(0.5),
92  m_pos_packets_timing_timeout(30.0),
93  m_device_ip(""),
94  m_pcap_verbose(true),
95  m_last_pos_packet_timestamp(INVALID_TIMESTAMP),
96  m_pcap(NULL),
97  m_pcap_out(NULL),
98  m_pcap_dumper(NULL),
99  m_pcap_bpf_program(NULL),
100  m_pcap_file_empty(true),
101  m_pcap_read_once(false),
102  m_pcap_read_fast(false),
103  m_pcap_read_full_scan_delay_ms(100),
104  m_pcap_repeat_delay(0.0),
105  m_hDataSock(INVALID_SOCKET),
106  m_hPositionSock(INVALID_SOCKET),
107  m_last_gps_rmc_age(INVALID_TIMESTAMP),
108  m_lidar_rpm(0),
109  m_lidar_return(UNCHANGED)
110 {
111  m_sensorLabel = "Velodyne";
112 
113 #if MRPT_HAS_LIBPCAP
114  m_pcap_bpf_program = new bpf_program[1];
115 #endif
116 
117 #ifdef MRPT_OS_WINDOWS
118  // Init the WinSock Library:
119  WORD wVersionRequested = MAKEWORD( 2, 0 );
120  WSADATA wsaData;
121  if (WSAStartup( wVersionRequested, &wsaData ) )
122  THROW_EXCEPTION("Error calling WSAStartup");
123 #endif
124 }
125 
127 {
128  this->close();
129 #ifdef MRPT_OS_WINDOWS
130  WSACleanup();
131 #endif
132 
133 #if MRPT_HAS_LIBPCAP
134  delete[] reinterpret_cast<bpf_program*>(m_pcap_bpf_program);
135  m_pcap_bpf_program=NULL;
136 #endif
137 
138 }
139 
140 bool CVelodyneScanner::loadCalibrationFile(const std::string & velodyne_xml_calib_file_path )
141 {
142  return m_velodyne_calib.loadFromXMLFile(velodyne_xml_calib_file_path);
143 }
144 
146  const mrpt::utils::CConfigFileBase &cfg,
147  const std::string &sect )
148 {
149  MRPT_START
150 
151  cfg.read_enum<CVelodyneScanner::model_t>(sect,"model",m_model);
152  MRPT_LOAD_HERE_CONFIG_VAR(device_ip, string, m_device_ip , cfg, sect);
153  MRPT_LOAD_HERE_CONFIG_VAR(pcap_input, string, m_pcap_input_file , cfg, sect);
154  MRPT_LOAD_HERE_CONFIG_VAR(pcap_output, string, m_pcap_output_file, cfg, sect);
155  MRPT_LOAD_HERE_CONFIG_VAR(pcap_read_once,bool, m_pcap_read_once, cfg, sect);
156  MRPT_LOAD_HERE_CONFIG_VAR(pcap_read_fast,bool, m_pcap_read_fast , cfg, sect);
157  MRPT_LOAD_HERE_CONFIG_VAR(pcap_read_full_scan_delay_ms,double, m_pcap_read_full_scan_delay_ms, cfg, sect);
158  MRPT_LOAD_HERE_CONFIG_VAR(pcap_repeat_delay,double, m_pcap_repeat_delay , cfg, sect);
159  MRPT_LOAD_HERE_CONFIG_VAR(pos_packets_timing_timeout,double, m_pos_packets_timing_timeout , cfg, sect);
160  MRPT_LOAD_HERE_CONFIG_VAR(pos_packets_min_period,double, m_pos_packets_min_period , cfg, sect);
161 
162  using mrpt::utils::DEG2RAD;
164  cfg.read_float(sect,"pose_x",0),
165  cfg.read_float(sect,"pose_y",0),
166  cfg.read_float(sect,"pose_z",0),
167  DEG2RAD( cfg.read_float(sect,"pose_yaw",0) ),
168  DEG2RAD( cfg.read_float(sect,"pose_pitch",0) ),
169  DEG2RAD( cfg.read_float(sect,"pose_roll",0) )
170  );
171 
172  std::string calibration_file;
173  MRPT_LOAD_CONFIG_VAR(calibration_file,string,cfg, sect);
174  if (!calibration_file.empty())
175  this->loadCalibrationFile(calibration_file);
176 
177  // Check validity:
179  if (lstModels.find(m_model)==lstModels.end()) {
180  THROW_EXCEPTION(mrpt::format("Unrecognized `model` parameter: `%u` . Known values are: %s",
181  static_cast<unsigned int>(m_model),
183  }
184 
185  // Optional HTTP-based settings:
186  MRPT_LOAD_HERE_CONFIG_VAR(rpm, int, m_lidar_rpm, cfg, sect);
187  m_lidar_return = cfg.read_enum<return_type_t>(sect, "return_type", m_lidar_return);
188 
189  MRPT_END
190 }
191 
193  mrpt::obs::CObservationVelodyneScanPtr & outScan,
194  mrpt::obs::CObservationGPSPtr & outGPS
195  )
196 {
197  try
198  {
199  ASSERTMSG_(m_initialized, "initialize() has not been called yet!");
200 
201  // Init with empty smart pointers:
202  outScan = mrpt::obs::CObservationVelodyneScanPtr();
203  outGPS = mrpt::obs::CObservationGPSPtr();
204 
205  // Try to get data & pos packets:
208  mrpt::system::TTimeStamp data_pkt_timestamp, pos_pkt_timestamp;
209  bool rx_all_ok = this->receivePackets(data_pkt_timestamp,rx_pkt, pos_pkt_timestamp,rx_pos_pkt);
210 
211  if (!rx_all_ok) {
212  // PCAP EOF:
213  return false;
214  }
215 
216  if (pos_pkt_timestamp!=INVALID_TIMESTAMP)
217  {
218  mrpt::obs::CObservationGPSPtr gps_obs = mrpt::obs::CObservationGPS::Create();
219  gps_obs->sensorLabel = this->m_sensorLabel + std::string("_GPS");
220  gps_obs->sensorPose = m_sensorPose;
221 
222  gps_obs->originalReceivedTimestamp = pos_pkt_timestamp;
223 
224  bool parsed_ok = CGPSInterface::parse_NMEA( std::string(rx_pos_pkt.NMEA_GPRMC), *gps_obs);
225  const mrpt::obs::gnss::Message_NMEA_RMC *msg_rmc = gps_obs->getMsgByClassPtr<mrpt::obs::gnss::Message_NMEA_RMC>();
226  if (!parsed_ok || !msg_rmc || msg_rmc->fields.validity_char!='A')
227  {
228  gps_obs->has_satellite_timestamp = false;
229  gps_obs->timestamp = pos_pkt_timestamp;
230  }
231  else
232  {
233  // We have live GPS signal and a recent RMC frame:
234  m_last_gps_rmc_age = pos_pkt_timestamp;
235  m_last_gps_rmc = *msg_rmc;
236  }
237  outGPS = gps_obs; // save in output object
238  }
239 
240  if (data_pkt_timestamp!=INVALID_TIMESTAMP)
241  {
242  m_state = ssWorking;
243 
244  // Break into a new observation object when the azimuth passes 360->0 deg:
245  const uint16_t rx_pkt_start_angle = rx_pkt.blocks[0].rotation;
246  //const uint16_t rx_pkt_end_angle = rx_pkt.blocks[CObservationVelodyneScan::BLOCKS_PER_PACKET-1].rotation;
247 
248  // Return the observation as done when a complete 360 deg scan is ready:
249  if (m_rx_scan && !m_rx_scan->scan_packets.empty())
250  {
251  if (rx_pkt_start_angle < m_rx_scan->scan_packets.rbegin()->blocks[0].rotation )
252  {
253  outScan = m_rx_scan;
254  m_rx_scan.clear_unique();
255 
256  if (m_pcap) {
257  // Keep the reader from blowing through the file.
258  if (!m_pcap_read_fast)
260  }
261  }
262  }
263 
264  // Create smart ptr to new in-progress observation:
265  if (!m_rx_scan) {
267  m_rx_scan->sensorLabel = this->m_sensorLabel + std::string("_SCAN");
268  m_rx_scan->sensorPose = m_sensorPose;
269  m_rx_scan->calibration = m_velodyne_calib; // Embed a copy of the calibration info
270 
271  {
273  model_properties_list_t::const_iterator it=lstModels.find(this->m_model);
274  if (it!=lstModels.end())
275  { // Model params:
276  m_rx_scan->maxRange = it->second.maxRange;
277  }
278  else // default params:
279  {
280  m_rx_scan->maxRange = 120.0;
281  }
282  }
283  }
284 
285  // For the first packet, set timestamp:
286  if (m_rx_scan->scan_packets.empty())
287  {
288  m_rx_scan->originalReceivedTimestamp = data_pkt_timestamp;
289  // Using GPS, if available:
291  {
292  // Each Velodyne data packet has a timestamp field,
293  // with the number of us since the top of the current HOUR:
294  // take the date and time from the GPS, then modify minutes and seconds from data pkt:
296 
297  mrpt::system::TTimeParts tim_parts;
298  mrpt::system::timestampToParts(gps_tim,tim_parts);
299  tim_parts.minute = rx_pkt.gps_timestamp /*us from top of hour*/ / 60000000ul;
300  tim_parts.second = (rx_pkt.gps_timestamp /*us from top of hour*/ % 60000000ul)*1e-6;
301 
302  const mrpt::system::TTimeStamp data_pkt_tim = mrpt::system::buildTimestampFromParts(tim_parts);
303 
304  m_rx_scan->timestamp = data_pkt_tim;
305  m_rx_scan->has_satellite_timestamp = true;
306  }
307  else
308  {
309  m_rx_scan->has_satellite_timestamp = false;
310  m_rx_scan->timestamp = data_pkt_timestamp;
311  }
312  }
313 
314  // Accumulate pkts in the observation object:
315  m_rx_scan->scan_packets.push_back(rx_pkt);
316  }
317 
318  return true;
319  }
320  catch(exception &e)
321  {
322  cerr << "[CVelodyneScanner::getObservation] Returning false due to exception: " << endl;
323  cerr << e.what() << endl;
324  return false;
325  }
326 }
327 
329 {
330  CObservationVelodyneScanPtr obs;
331  CObservationGPSPtr obs_gps;
332 
333  if (getNextObservation(obs,obs_gps))
334  {
335  m_state = ssWorking;
336  if (obs) appendObservation( obs );
337  if (obs_gps) appendObservation( obs_gps );
338  }
339  else
340  {
341  m_state = ssError;
342  cerr << "ERROR receiving data from Velodyne devic!" << endl;
343  }
344 }
345 
346 /** Tries to initialize the sensor, after setting all the parameters with a call to loadConfig.
347 * \exception This method must throw an exception with a descriptive message if some critical error is found. */
349 {
350  this->close();
351 
352  // (0) Preparation:
353  // --------------------------------
354  // Make sure we have calibration data:
355  if (m_velodyne_calib.empty()) {
356  // Try to load default data:
357  m_velodyne_calib = VelodyneCalibration::LoadDefaultCalibration( mrpt::utils::TEnumType<CVelodyneScanner::model_t>::value2name(m_model) );
358  if (m_velodyne_calib.empty())
359  THROW_EXCEPTION("Could not find default calibration data for the given LIDAR `model` name. Please, specify a valid `model` or load a valid XML configuration file first.");
360  }
361 
362  // online vs off line operation??
363  // -------------------------------
364  if (m_pcap_input_file.empty())
365  { // Online
366 
367  if (m_lidar_rpm>0) {
368  if (!setLidarRPM(m_lidar_rpm))
369  THROW_EXCEPTION("Error in setLidarRPM()!");
370  }
371  if (m_lidar_return != UNCHANGED) {
373  THROW_EXCEPTION("Error in setLidarReturnType()!");
374  }
375 
376  // (1) Create LIDAR DATA socket
377  // --------------------------------
378  if ( INVALID_SOCKET == (m_hDataSock = socket(PF_INET, SOCK_DGRAM, 0)) )
379  THROW_EXCEPTION( format("Error creating UDP socket:\n%s", mrpt::utils::net::getLastSocketErrorStr().c_str() ));
380 
381  struct sockaddr_in bindAddr;
382  memset(&bindAddr,0,sizeof(bindAddr));
383  bindAddr.sin_family = AF_INET;
384  bindAddr.sin_port = htons(VELODYNE_DATA_UDP_PORT);
385  bindAddr.sin_addr.s_addr = INADDR_ANY;
386 
387  if( int(INVALID_SOCKET) == ::bind(m_hDataSock,(struct sockaddr *)(&bindAddr),sizeof(sockaddr)) )
389 
390 #ifdef MRPT_OS_WINDOWS
391  unsigned long non_block_mode = 1;
392  if (ioctlsocket(m_hDataSock, FIONBIO, &non_block_mode) ) THROW_EXCEPTION( "Error entering non-blocking mode with ioctlsocket()." );
393 #else
394  int oldflags=fcntl(m_hDataSock, F_GETFL, 0);
395  if (oldflags == -1) THROW_EXCEPTION( "Error retrieving fcntl() of socket." );
396  oldflags |= O_NONBLOCK | FASYNC;
397  if (-1==fcntl(m_hDataSock, F_SETFL, oldflags)) THROW_EXCEPTION( "Error entering non-blocking mode with fcntl()." );
398 #endif
399 
400  // (2) Create LIDAR POSITION socket
401  // --------------------------------
402  if ( INVALID_SOCKET == (m_hPositionSock = socket(PF_INET, SOCK_DGRAM, 0)) )
403  THROW_EXCEPTION( format("Error creating UDP socket:\n%s", mrpt::utils::net::getLastSocketErrorStr().c_str() ));
404 
405  bindAddr.sin_port = htons(VELODYNE_POSITION_UDP_PORT);
406 
407  if( int(INVALID_SOCKET) == ::bind(m_hPositionSock,(struct sockaddr *)(&bindAddr),sizeof(sockaddr)) )
409 
410 #ifdef MRPT_OS_WINDOWS
411  if (ioctlsocket(m_hPositionSock, FIONBIO, &non_block_mode) ) THROW_EXCEPTION( "Error entering non-blocking mode with ioctlsocket()." );
412 #else
413  oldflags=fcntl(m_hPositionSock, F_GETFL, 0);
414  if (oldflags == -1) THROW_EXCEPTION( "Error retrieving fcntl() of socket." );
415  oldflags |= O_NONBLOCK | FASYNC;
416  if (-1==fcntl(m_hPositionSock, F_SETFL, oldflags)) THROW_EXCEPTION( "Error entering non-blocking mode with fcntl()." );
417 #endif
418  }
419  else
420  { // Offline:
421 #if MRPT_HAS_LIBPCAP
422  char errbuf[PCAP_ERRBUF_SIZE];
423 
424  if (m_pcap_verbose) printf("\n[CVelodyneScanner] Opening PCAP file \"%s\"\n", m_pcap_input_file.c_str());
425  if ((m_pcap = pcap_open_offline(m_pcap_input_file.c_str(), errbuf) ) == NULL) {
426  THROW_EXCEPTION_FMT("Error opening PCAP file: '%s'",errbuf);
427  }
428 
429  // Build PCAP filter:
430  {
431  std::string filter_str =
432  mrpt::format("(udp dst port %d || udp dst port %d)",
435  if( !m_device_ip.empty() )
436  filter_str += "&& src host " + m_device_ip;
437 
438  static std::string sMsgError = "[CVelodyneScanner] Error calling pcap_compile: "; // This is to avoid the ill-formed signature of pcap_error() accepting "char*", not "const char*"... sigh
439  if (pcap_compile( reinterpret_cast<pcap_t*>(m_pcap), reinterpret_cast<bpf_program*>(m_pcap_bpf_program), filter_str.c_str(), 1, PCAP_NETMASK_UNKNOWN) <0)
440  pcap_perror(reinterpret_cast<pcap_t*>(m_pcap), &sMsgError[0] );
441  }
442 
443  m_pcap_file_empty = true;
444  m_pcap_read_count = 0;
445 
446 #else
447  THROW_EXCEPTION("Velodyne: Reading from PCAP requires building MRPT with libpcap support!");
448 #endif
449  }
450 
454 
455  m_initialized=true;
456 }
457 
459 {
461  {
462  shutdown(m_hDataSock, 2 ); //SD_BOTH );
463 #ifdef MRPT_OS_WINDOWS
464  closesocket( m_hDataSock );
465 #else
466  ::close( m_hDataSock );
467 #endif
469  }
470 
472  {
473  shutdown(m_hPositionSock, 2 ); //SD_BOTH );
474 #ifdef MRPT_OS_WINDOWS
475  closesocket( m_hPositionSock );
476 #else
478 #endif
480  }
481 #if MRPT_HAS_LIBPCAP
482  if (m_pcap) {
483  pcap_close( reinterpret_cast<pcap_t*>(m_pcap) );
484  m_pcap = NULL;
485  }
486  if (m_pcap_dumper) {
487  pcap_dump_close(reinterpret_cast<pcap_dumper_t*>(m_pcap_dumper));
488  m_pcap_dumper=NULL;
489  }
490  if (m_pcap_out) {
491  pcap_close( reinterpret_cast<pcap_t*>(m_pcap_out) );
492  m_pcap_out = NULL;
493  }
494 #endif
495  m_initialized=false;
496 }
497 
498 // Fixed Ethernet headers for PCAP capture --------
499 #if MRPT_HAS_LIBPCAP
500 const uint16_t LidarPacketHeader[21] = {
501  0xffff, 0xffff, 0xffff, 0x7660,
502  0x0088, 0x0000, 0x0008, 0x0045,
503  0xd204, 0x0000, 0x0040, 0x11ff,
504  0xaab4, 0xa8c0, 0xc801, 0xffff, // checksum 0xa9b4 //source ip 0xa8c0, 0xc801 is 192.168.1.200
505  0xffff, 0x4009, 0x4009, 0xbe04, 0x0000};
506 const uint16_t PositionPacketHeader[21] = {
507  0xffff, 0xffff, 0xffff, 0x7660,
508  0x0088, 0x0000, 0x0008, 0x0045,
509  0xd204, 0x0000, 0x0040, 0x11ff,
510  0xaab4, 0xa8c0, 0xc801, 0xffff, // checksum 0xa9b4 //source ip 0xa8c0, 0xc801 is 192.168.1.200
511  0xffff, 0x7420, 0x7420, 0x0802, 0x0000};
512 #endif
513 
514 #if defined(MRPT_OS_WINDOWS) && MRPT_HAS_LIBPCAP
515 int gettimeofday(struct timeval * tp, void *)
516 {
517  FILETIME ft;
518  ::GetSystemTimeAsFileTime( &ft );
519  long long t = (static_cast<long long>(ft.dwHighDateTime) << 32) | ft.dwLowDateTime;
520  t -= 116444736000000000LL;
521  t /= 10; // microseconds
522  tp->tv_sec = static_cast<long>( t / 1000000UL);
523  tp->tv_usec = static_cast<long>( t % 1000000UL);
524  return 0;
525 }
526 #endif
527 
529  mrpt::system::TTimeStamp &data_pkt_timestamp,
531  mrpt::system::TTimeStamp &pos_pkt_timestamp,
533  )
534 {
535  MRPT_COMPILE_TIME_ASSERT(sizeof(mrpt::obs::CObservationVelodyneScan::TVelodynePositionPacket)== CObservationVelodyneScan::POS_PACKET_SIZE);
536  MRPT_COMPILE_TIME_ASSERT(sizeof(mrpt::obs::CObservationVelodyneScan::TVelodyneRawPacket)== CObservationVelodyneScan::PACKET_SIZE);
537 
538  bool ret = true; // all ok
539  if (m_pcap)
540  {
542  data_pkt_timestamp, (uint8_t*)&out_data_pkt,
543  pos_pkt_timestamp, (uint8_t*)&out_pos_pkt);
544  }
545  else
546  {
547  data_pkt_timestamp = internal_receive_UDP_packet(m_hDataSock ,(uint8_t*)&out_data_pkt, CObservationVelodyneScan::PACKET_SIZE,m_device_ip);
548  pos_pkt_timestamp = internal_receive_UDP_packet(m_hPositionSock ,(uint8_t*)&out_pos_pkt, CObservationVelodyneScan::POS_PACKET_SIZE,m_device_ip);
549  }
550 
551  // Optional PCAP dump:
552 #if MRPT_HAS_LIBPCAP
553  // Save to PCAP file?
554  if (!m_pcap_output_file.empty() && !m_pcap_out) // 1st time: create output file
555  {
556 #if MRPT_HAS_LIBPCAP
557  char errbuf[PCAP_ERRBUF_SIZE];
558 
561  string sFilePostfix = "_";
562  sFilePostfix += format("%04u-%02u-%02u_%02uh%02um%02us",(unsigned int)parts.year, (unsigned int)parts.month, (unsigned int)parts.day, (unsigned int)parts.hour, (unsigned int)parts.minute, (unsigned int)parts.second );
563  const string sFileName = m_pcap_output_file + mrpt::system::fileNameStripInvalidChars( sFilePostfix ) + string(".pcap");
564 
565  m_pcap_out = pcap_open_dead(DLT_EN10MB, 65535);
566  ASSERTMSG_(m_pcap_out!=NULL, "Error creating PCAP live capture handle");
567 
568  printf("\n[CVelodyneScanner] Writing to PCAP file \"%s\"\n", sFileName.c_str());
569  if ((m_pcap_dumper = pcap_dump_open(reinterpret_cast<pcap_t*>(m_pcap_out),sFileName.c_str())) == NULL) {
570  THROW_EXCEPTION_FMT("Error creating PCAP live dumper: '%s'",errbuf);
571  }
572 #else
573  THROW_EXCEPTION("Velodyne: Writing PCAP files requires building MRPT with libpcap support!");
574 #endif
575  }
576 
577 
578  if (m_pcap_out && m_pcap_dumper && (data_pkt_timestamp!=INVALID_TIMESTAMP || pos_pkt_timestamp!=INVALID_TIMESTAMP))
579  {
580  struct pcap_pkthdr header;
581  struct timeval currentTime;
582  gettimeofday(&currentTime, NULL);
583  std::vector<unsigned char> packetBuffer;
584 
585  // Data pkt:
586  if (data_pkt_timestamp!=INVALID_TIMESTAMP)
587  {
588  header.caplen = header.len = CObservationVelodyneScan::PACKET_SIZE+42;
589  packetBuffer.resize(header.caplen);
590  header.ts = currentTime;
591 
592  memcpy(&(packetBuffer[0]), LidarPacketHeader, 42);
593  memcpy(&(packetBuffer[0]) + 42, (uint8_t*)&out_data_pkt, CObservationVelodyneScan::PACKET_SIZE);
594  pcap_dump((u_char*)this->m_pcap_dumper, &header, &(packetBuffer[0]));
595  }
596  // Pos pkt:
597  if (pos_pkt_timestamp!=INVALID_TIMESTAMP)
598  {
599  header.caplen = header.len = CObservationVelodyneScan::POS_PACKET_SIZE+42;
600  packetBuffer.resize(header.caplen);
601  header.ts = currentTime;
602 
603  memcpy(&(packetBuffer[0]), PositionPacketHeader, 42);
604  memcpy(&(packetBuffer[0]) + 42, (uint8_t*)&out_pos_pkt, CObservationVelodyneScan::POS_PACKET_SIZE);
605  pcap_dump((u_char*)this->m_pcap_dumper, &header, &(packetBuffer[0]));
606  }
607  }
608 #endif
609 
610  // Convert from Velodyne's standard little-endian ordering to host byte ordering:
611  // (done AFTER saving the pckg as is to pcap above)
612 #if MRPT_IS_BIG_ENDIAN
613  if (data_pkt_timestamp!=INVALID_TIMESTAMP)
614  {
616  for (int i=0;i<CObservationVelodyneScan::BLOCKS_PER_PACKET;i++)
617  {
620  for (int k=0;k<CObservationVelodyneScan::SCANS_PER_BLOCK;k++) {
622  }
623  }
624  }
625  if (pos_pkt_timestamp!=INVALID_TIMESTAMP)
626  {
629  }
630 #endif
631 
632 
633  // Position packet decimation:
634  if (pos_pkt_timestamp!=INVALID_TIMESTAMP) {
637  // Ignore this packet
638  pos_pkt_timestamp = INVALID_TIMESTAMP;
639  }
640  else {
641  // Reset time watch:
642  m_last_pos_packet_timestamp = pos_pkt_timestamp;
643  }
644  }
645 
646  return ret;
647 }
648 
649 // static method:
651  platform_socket_t hSocket,
652  uint8_t * out_buffer,
653  const size_t expected_packet_size,
654  const std::string &filter_only_from_IP)
655 {
656  if (hSocket==INVALID_SOCKET)
657  THROW_EXCEPTION("Error: UDP socket is not open yet! Have you called initialize() first?");
658 
659  unsigned long devip_addr = 0;
660  if (!filter_only_from_IP.empty())
661  devip_addr = inet_addr(filter_only_from_IP.c_str());
662 
664 
665  struct pollfd fds[1];
666  fds[0].fd = hSocket;
667  fds[0].events = POLLIN;
668  static const int POLL_TIMEOUT = 1; // (ms)
669 
670  sockaddr_in sender_address;
671  socklen_t sender_address_len = sizeof(sender_address);
672 
673  while (true)
674  {
675  // Unfortunately, the Linux kernel recvfrom() implementation
676  // uses a non-interruptible sleep() when waiting for data,
677  // which would cause this method to hang if the device is not
678  // providing data. We poll() the device first to make sure
679  // the recvfrom() will not block.
680  //
681  // Note, however, that there is a known Linux kernel bug:
682  //
683  // Under Linux, select() may report a socket file descriptor
684  // as "ready for reading", while nevertheless a subsequent
685  // read blocks. This could for example happen when data has
686  // arrived but upon examination has wrong checksum and is
687  // discarded. There may be other circumstances in which a
688  // file descriptor is spuriously reported as ready. Thus it
689  // may be safer to use O_NONBLOCK on sockets that should not
690  // block.
691 
692  // poll() until input available
693  do
694  {
695  int retval =
696 #if !defined(MRPT_OS_WINDOWS)
697  poll
698 #else
699  WSAPoll
700 #endif
701  (fds, 1, POLL_TIMEOUT);
702  if (retval < 0) // poll() error?
703  {
704  if (errno != EINTR)
705  THROW_EXCEPTION( format("Error in UDP poll():\n%s", mrpt::utils::net::getLastSocketErrorStr().c_str() ));
706  }
707  if (retval == 0) // poll() timeout?
708  {
709  return INVALID_TIMESTAMP;
710  }
711  if ((fds[0].revents & POLLERR)
712  || (fds[0].revents & POLLHUP)
713  || (fds[0].revents & POLLNVAL)) // device error?
714  {
715  THROW_EXCEPTION( "Error in UDP poll() (seems Velodyne device error?)");
716  }
717  } while ((fds[0].revents & POLLIN) == 0);
718 
719  // Receive packets that should now be available from the
720  // socket using a blocking read.
721  int nbytes = recvfrom(hSocket, (char*)&out_buffer[0],
722  expected_packet_size, 0,
723  (sockaddr*) &sender_address, &sender_address_len);
724 
725  if (nbytes < 0)
726  {
727  if (errno != EWOULDBLOCK)
728  THROW_EXCEPTION("recvfrom() failed!?!")
729  }
730  else if ((size_t) nbytes == expected_packet_size)
731  {
732  // read successful,
733  // if packet is not from the lidar scanner we selected by IP,
734  // continue otherwise we are done
735  if( !filter_only_from_IP.empty() && sender_address.sin_addr.s_addr != devip_addr )
736  continue;
737  else
738  break; //done
739  }
740 
741  std::cerr << "[CVelodyneScanner] Warning: incomplete Velodyne packet read: " << nbytes << " bytes\n";
742  }
743 
744  // Average the times at which we begin and end reading. Use that to
745  // estimate when the scan occurred.
747 
748  return (time1/2+time2/2);
749 }
750 
752  mrpt::system::TTimeStamp & data_pkt_time, uint8_t *out_data_buffer,
753  mrpt::system::TTimeStamp & pos_pkt_time, uint8_t *out_pos_buffer
754  )
755 {
756 #if MRPT_HAS_LIBPCAP
757  ASSERT_(m_pcap);
758 
759  data_pkt_time = INVALID_TIMESTAMP;
760  pos_pkt_time = INVALID_TIMESTAMP;
761 
762  char errbuf[PCAP_ERRBUF_SIZE];
763  struct pcap_pkthdr *header;
764  const u_char *pkt_data;
765 
766  while (true)
767  {
768  int res;
769  if ((res = pcap_next_ex(reinterpret_cast<pcap_t*>(m_pcap), &header, &pkt_data)) >= 0)
770  {
772 
773  // if packet is not from the lidar scanner we selected by IP, continue
774  if(pcap_offline_filter( reinterpret_cast<bpf_program*>(m_pcap_bpf_program), header, pkt_data ) == 0)
775  {
776  if (m_verbose) std::cout << "[CVelodyneScanner] DEBUG: Filtering out packet #"<< m_pcap_read_count <<" in PCAP file.\n";
777  continue;
778  }
779 
780  // Determine whether it is a DATA or POSITION packet:
781  m_pcap_file_empty = false;
783  const uint16_t udp_dst_port = ntohs( *reinterpret_cast<const uint16_t *>(pkt_data + 0x24) );
784 
786  if (m_verbose) std::cout << "[CVelodyneScanner] DEBUG: Packet #"<< m_pcap_read_count <<" in PCAP file is POSITION pkt.\n";
787  memcpy(out_pos_buffer, pkt_data+42, CObservationVelodyneScan::POS_PACKET_SIZE);
788  pos_pkt_time = tim; // success
789  return true;
790  }
791  else if (udp_dst_port==CVelodyneScanner::VELODYNE_DATA_UDP_PORT) {
792  if (m_verbose) std::cout << "[CVelodyneScanner] DEBUG: Packet #"<< m_pcap_read_count <<" in PCAP file is DATA pkt.\n";
793  memcpy(out_data_buffer, pkt_data+42, CObservationVelodyneScan::PACKET_SIZE);
794  data_pkt_time = tim; // success
795  return true;
796  }
797  else {
798  std::cerr << "[CVelodyneScanner] ERROR: Packet "<<m_pcap_read_count <<" in PCAP file passed the filter but does not match expected UDP port numbers! Skipping it.\n";
799  }
800  }
801 
802  if (m_pcap_file_empty) // no data in file?
803  {
804  fprintf(stderr, "[CVelodyneScanner] Maybe the PCAP file is empty? Error %d reading Velodyne packet: `%s`\n", res, pcap_geterr(reinterpret_cast<pcap_t*>(m_pcap) ));
805  return true;
806  }
807 
808  if (m_pcap_read_once)
809  {
810  if (m_pcap_verbose) printf("[CVelodyneScanner] INFO: end of file reached -- done reading.\n");
811  mrpt::system::sleep(250);
812  return false;
813  }
814 
815  if (m_pcap_repeat_delay > 0.0)
816  {
817  if (m_pcap_verbose) printf("[CVelodyneScanner] INFO: end of file reached -- delaying %.3f seconds.\n", m_pcap_repeat_delay);
819  }
820 
821  if (m_pcap_verbose) printf("[CVelodyneScanner] INFO: replaying Velodyne dump file.\n");
822 
823  // rewind the file
824  pcap_close( reinterpret_cast<pcap_t*>(m_pcap) );
825  if ((m_pcap = pcap_open_offline(m_pcap_input_file.c_str(), errbuf) ) == NULL) {
826  THROW_EXCEPTION_FMT("Error opening PCAP file: '%s'",errbuf);
827  }
828  m_pcap_file_empty = true; // maybe the file disappeared?
829  } // loop back and try again
830 #else
831  THROW_EXCEPTION("MRPT needs to be built against libpcap to enable this functionality")
832 #endif
833 }
834 
836 {
837  /* HTTP-based config: http://10.0.0.100/tab/config.html
838  <form name='returns' method="post" action="/cgi/setting">
839  <span>Return&nbsp;Type:&nbsp;</span> <select name="returns"
840  onchange='javascript:this.form.submit()'>
841  <option>Strongest</option>
842  <option>Last</option>
843  <option>Dual</option>
844  </select>
845  </form>
846  */
847  MRPT_START;
848  std::string strRet;
849  switch (ret_type)
850  {
851  case STRONGEST: strRet = "Strongest"; break;
852  case DUAL: strRet = "Dual"; break;
853  case LAST: strRet = "Last"; break;
854  case UNCHANGED: return true;
855  default:
856  THROW_EXCEPTION("Invalid value for return type!");
857  };
858 
859  const std::string cmd = mrpt::format("returns=%s", strRet.c_str());
860  return this->internal_send_http_post(cmd);
861  MRPT_END;
862 }
863 
865 {
866  /* HTTP-based config: http://10.0.0.100/tab/config.html
867  <form name='rpm' method="post" action="/cgi/setting">
868  Motor
869  &nbsp;RPM:&nbsp;<input type="text" name="rpm" size="5" style="text-align:right" /><input type="button" value="+" onclick="javascript:this.form.rpm.value++;this.form.submit()" /><input type="button" value="-" onclick="javascript:this.form.rpm.value--;this.form.submit()" />
870  &nbsp;<input type="submit" value="Set" />
871  </form>
872  */
873 
874  MRPT_START;
875  const std::string cmd = mrpt::format("rpm=%i", rpm);
876  return this->internal_send_http_post(cmd);
877  MRPT_END;
878 }
879 
881 {
882  // laser = on|off
883  MRPT_START;
884  const std::string cmd = mrpt::format("laser=%s", on ? "on":"off");
885  return this->internal_send_http_post(cmd);
886  MRPT_END;
887 }
888 
890 {
891  MRPT_START;
892 
893  ASSERTMSG_(!m_device_ip.empty(), "A device IP address must be specified first!");
894 
895  using namespace mrpt::utils::net;
896 
897  vector_byte post_out;
898  string post_err_str;
899 
900  int http_rep_code;
901  mrpt::utils::TParameters<string> extra_headers, out_headers;
902 
903  extra_headers["Origin"] = mrpt::format("http://%s", m_device_ip.c_str());
904  extra_headers["Referer"] = mrpt::format("http://%s", m_device_ip.c_str());
905  extra_headers["Upgrade-Insecure-Requests"] = "1";
906  extra_headers["Content-Type"] = "application/x-www-form-urlencoded";
907 
909  "POST",
910  post_data,
911  mrpt::format("http://%s/cgi/setting", m_device_ip.c_str()),
912  post_out,
913  post_err_str,
914  80 /* port */,
915  string(), string(), // user,pass
916  &http_rep_code,
917  &extra_headers,
918  &out_headers
919  );
920 
921  return mrpt::utils::net::erOk==ret && (http_rep_code == 200 || http_rep_code == 204); // OK codes
922 
923  MRPT_END;
924 }
925 
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
std::map< model_t, TModelProperties > model_properties_list_t
ERRORCODE_HTTP
Possible returns from a HTTP request.
Definition: net_utils.h:31
void BASE_IMPEXP 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:101
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.
GLdouble GLdouble t
Definition: glext.h:3610
unsigned __int16 uint16_t
Definition: rptypes.h:46
For usage when passing a dynamic number of (numeric) arguments to a function, by name.
Definition: TParameters.h:51
A C++ interface to Velodyne laser scanners (HDL-64, HDL-32, VLP-16), working on Linux and Windows...
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. Change it if required.
std::vector< uint8_t > vector_byte
Definition: types_simple.h:26
double DEG2RAD(const double x)
Degrees to radians.
void BASE_IMPEXP reverseBytesInPlace(bool &v_in_out)
Reverse the order of the bytes of a given type (useful for transforming btw little/big endian) ...
int platform_socket_t
Handles for the UDP sockets, or INVALID_SOCKET (-1)
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
std::string m_sensorLabel
See CGenericSensor.
#define THROW_EXCEPTION(msg)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
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 BASE_IMPEXP buildTimestampFromParts(const mrpt::system::TTimeParts &p)
Builds a timestamp from the parts (Parts are in UTC)
Definition: datetime.cpp:127
static std::string getListKnownModels()
Return human-readable string: "`VLP16`,`XXX`,...".
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:70
Contains classes for various device interfaces.
void * m_pcap_bpf_program
opaque ptr: bpf_program*
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:412
STL namespace.
std::string BASE_IMPEXP 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:315
const Scalar * const_iterator
Definition: eigen_plugins.h:24
static CObservationGPSPtr Create()
GLdouble s
Definition: glext.h:3602
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
void loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase &configSource, const std::string &section)
See the class documentation at the top for expected parameters.
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"
This class allows loading and storing values and vectors of different types from a configuration text...
unsigned char uint8_t
Definition: rptypes.h:43
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)
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.
#define MRPT_END
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...
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
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:35
std::string BASE_IMPEXP getLastSocketErrorStr()
Returns a description of the last Sockets error.
Definition: net_utils.cpp:519
static CObservationVelodyneScanPtr Create()
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:39
static const model_properties_list_t & get()
Singleton access.
This namespace contains representation of robot actions and observations.
A set of useful routines for networking.
Definition: net_utils.h:23
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
bool m_pcap_verbose
Default: true Output PCAP Info msgs.
#define INVALID_SOCKET
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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:3919
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...
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...
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
double time2
double second
Minute (0-59)
Definition: datetime.h:42
#define MRPT_COMPILE_TIME_ASSERT(f)
void * m_pcap_out
opaque ptr: "pcap_t*"
bool internal_send_http_post(const std::string &post_data)
#define MRPT_START
mrpt::system::TTimeStamp getDateAsTimestamp() const
Build an MRPT timestamp with the year/month/day of this observation.
#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...
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:41
#define MRPT_LOAD_HERE_CONFIG_VAR(variableName, variableType, targetVariable, configFileObject, sectionNameStr)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
One unit of data from the scanner (the payload of one UDP DATA packet)
bool getNextObservation(mrpt::obs::CObservationVelodyneScanPtr &outScan, mrpt::obs::CObservationGPSPtr &outGPS)
Polls the UDP port for incoming data packets.
bool empty() const
Returns true if no calibration has been loaded yet.
static short int VELODYNE_DATA_UDP_PORT
Default: 2368. Change it if required.
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:38
#define ASSERT_(f)
ERRORCODE_HTTP BASE_IMPEXP http_request(const string &http_method, const string &http_send_content, const string &url, vector_byte &out_content, string &out_errormsg, int port=80, const string &auth_user=string(), const string &auth_pass=string(), int *out_http_responsecode=NULL, mrpt::utils::TParameters< string > *extra_headers=NULL, mrpt::utils::TParameters< string > *out_headers=NULL, int timeout_ms=1000)
Generic function for HTTP GET & POST methods.
Definition: net_utils.cpp:78
void doProcess()
This method will be invoked at a minimum rate of "process_rate" (Hz)
void appendObservation(const mrpt::utils::CSerializablePtr &obj)
Like appendObservations() but for just one observation.
uint8_t hour
Day (1-31)
Definition: datetime.h:40
uint16_t rotation
0-35999, divide by 100 to get degrees
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
GLuint res
Definition: glext.h:6298
void * m_pcap_dumper
opaque ptr: "pcap_dumper_t *"
std::string m_device_ip
Default: "" (no IP-based filtering)
mrpt::obs::CObservationVelodyneScanPtr m_rx_scan
In progress RX scan.
bool loadCalibrationFile(const std::string &velodyne_xml_calib_file_path)
Returns false on error.
#define ASSERTMSG_(f, __ERROR_MSG)
mrpt::system::TTimeStamp m_last_gps_rmc_age
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
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.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020