MRPT  1.9.9
CVelodyneScanner.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, 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 #pragma once
11 
17 
18 namespace mrpt::hwdrivers
19 {
20 /** A C++ interface to Velodyne laser scanners (HDL-64, HDL-32, VLP-16), working
21  * on Linux and Windows.
22  * (Using this class requires WinPCap as a run-time dependency in Windows).
23  * It can receive data from real devices via an Ethernet connection or parse a
24  * WireShark PCAP file for offline processing.
25  * The choice of online vs. offline operation is taken upon calling \a
26  * initialize(): if a PCAP input file has been defined,
27  * offline operation takes place and network is not listened for incomming
28  * packets.
29  *
30  * Parsing dual return scans requires a VLP-16 with firmware version 3.0.23 or
31  * newer. While converting the scan into a
32  * point cloud in mrpt::obs::CObservationVelodyneScan you can select whether to
33  * keep the strongest, the last or both laser returns.
34  *
35  * XML calibration files are not mandatory for VLP-16 and HDL-32, but they are
36  * for HDL-64.
37  *
38  * <h2>Grabbing live data (as a user)</h2> <hr>
39  * - Use the application
40  * [velodyne-view](http://www.mrpt.org/list-of-mrpt-apps/application-velodyne-view/)
41  * to visualize the LIDAR output in real-time (optionally saving to a PCAP file)
42  * or to playback a PCAP file.
43  * - Use
44  * [rawlog-grabber](http://www.mrpt.org/list-of-mrpt-apps/application-rawlog-grabber/)
45  * to record a dataset in MRPT's format together with any other set of sensors.
46  * See example config file:
47  * [MRPT\share\mrpt\config_files\rawlog-grabber\velodyne.ini](https://github.com/MRPT/mrpt/blob/master/share/mrpt/config_files/rawlog-grabber/velodyne.ini)
48  *
49  * <h2>Grabbing live data (programmatically)</h2> <hr>
50  * - See CGenericSensor for a general overview of the sequence of methods to
51  * be called: loadConfig(), initialize(), doProcess().
52  * - Or use this class inside the application
53  * [rawlog-grabber](http://www.mrpt.org/list-of-mrpt-apps/application-rawlog-grabber/).
54  * See example config files:
55  * [MRPT\share\mrpt\config_files\rawlog-grabber\velodyne.ini](https://github.com/MRPT/mrpt/blob/master/share/mrpt/config_files/rawlog-grabber/velodyne.ini)
56  *
57  * See the source code of the example application `[MRPT]/apps/velodyne-view`
58  * ([velodyne-view web
59  * page](http://www.mrpt.org/list-of-mrpt-apps/application-velodyne-view/)) for
60  * more details.
61  *
62  * <h2>Playing back a PCAP file:</h2><hr>
63  * It is common to save Velodyne datasets as Wireshark's PCAP files.
64  * These files can be played back with tools like
65  * [bittwist](http://bittwist.sourceforge.net/), which emit all UDP packets in
66  * the PCAP log.
67  * Then, use this class to receive the packets as if they come from the real
68  * sensor.
69  *
70  * Alternatively, if MRPT is linked against libpcap, this class can directly
71  * parse a PCAP file to simulate reading from a device offline.
72  * See method setPCAPInputFile() and config file parameter ``
73  *
74  * To compile with PCAP support: In Debian/Ubuntu, install libpcap-dev. In
75  * Windows, install WinPCap developer packages + the regular WinPCap driver.
76  *
77  * <h2>Configuration and usage:</h2> <hr>
78  * Data is returned as observations of type:
79  * - mrpt::obs::CObservationVelodyneScan for one or more "data packets" (refer
80  * to Velodyne usage manual)
81  * - mrpt::obs::CObservationGPS for GPS (GPRMC) packets, if available via the
82  * synchronization interface of the device.
83  * See those classes for documentation on their fields.
84  *
85  * Configuration includes setting the device IP (optional) and sensor model
86  * (mandatory only if a calibration file is not provided).
87  * These parameters can be set programmatically (see methods of this class), or
88  * via a configuration file with CGenericSensor::loadConfig() (see example
89  * config file section below).
90  *
91  * <h2>About timestamps:</h2><hr>
92  * Each gathered observation of type mrpt::obs::CObservationVelodyneScan is
93  * populated with two timestamps, one for the local PC timestamp and,
94  * if available, another one for the GPS-stamped timestamp. Refer to the
95  * observation docs for details.
96  *
97  * <h2>Format of parameters for loading from a .ini file</h2><hr>
98  * \code
99  * PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS:
100  * -------------------------------------------------------
101  * [supplied_section_name]
102  * # ---- Sensor description ----
103  * #calibration_file = PUT_HERE_FULL_PATH_TO_CALIB_FILE.xml //
104  * Optional but recommended: put here your vendor-provided calibration file
105  * model = VLP16 // Can be any of: `VLP16`, `HDL32`,
106  * `HDL64` (It is used to load default calibration file. Parameter not required
107  * if `calibration_file` is provided.
108  * #pos_packets_min_period = 0.5 // (Default=0.5 seconds) Minimum
109  * period to leave between reporting position packets. Used to decimate the
110  * large number of packets of this type.
111  * # How long to wait, after loss of GPS signal, to report timestamps as "not
112  * based on satellite time". 30 secs, with typical velodyne clock drifts, means
113  * a ~1.7 ms typical drift.
114  * #pos_packets_timing_timeout = 30 // (Default=30 seconds)
115  * # ---- Online operation ----
116  *
117  * # IP address of the device. UDP packets from other IPs will be ignored.
118  * Leave commented or blank
119  * # if only one scanner is present (no IP filtering)
120  * #device_ip = XXX.XXX.XXX.XXX
121  *
122  * #rpm = 600 // Sensor RPM (Default: unchanged). Requires
123  * setting `device_ip`
124  * #return_type = STRONGEST // Any of: 'STRONGEST', 'LAST', 'DUAL'
125  * (Default: unchanged). Requires setting `device_ip`
126  *
127  * # ---- Offline operation ----
128  * # If uncommented, this class will read from the PCAP instead of connecting
129  * and listeling
130  * # for online network packets.
131  * # pcap_input = PUT_FULL_PATH_TO_PCAP_LOG_FILE.pcap
132  * # pcap_read_once = false // Do not loop
133  * # pcap_read_fast = false // fast forward skipping non-velodyne packets
134  * # pcap_read_full_scan_delay_ms = 100 // Used to simulate a reasonable
135  * number of full scans / second
136  * # pcap_repeat_delay = 0.0 // seconds
137  *
138  * # ---- Save to PCAP file ----
139  * # If uncommented, a PCAP file named
140  * `[pcap_output_prefix]_[DATE_TIME].pcap` will be
141  * # written simultaneously to the normal operation of this class.
142  * # pcap_output = velodyne_log
143  *
144  * # 3D position of the sensor on the vehicle:
145  * pose_x = 0 // 3D position (meters)
146  * pose_y = 0
147  * pose_z = 0
148  * pose_yaw = 0 // 3D orientation (degrees)
149  * pose_pitch = 0
150  * pose_roll = 0
151  *
152  * \endcode
153  *
154  *
155  * <h2>Copyright notice</h2><hr>
156  * Portions of this class are based on code from velodyne ROS node in
157  * https://github.com/ros-drivers/velodyne
158  * Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
159  * Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
160  * License: Modified BSD Software License Agreement
161  *
162  * \note New in MRPT 1.4.0
163  * \ingroup mrpt_hwdrivers_grp
164  */
166 {
168  public:
169  /** Default: 2368. Change it if required. */
170  static short int VELODYNE_DATA_UDP_PORT;
171  /** Default: 8308. Change it if required. */
172  static short int VELODYNE_POSITION_UDP_PORT;
173 
174  /** LIDAR model types */
175  enum model_t
176  {
177  VLP16 = 1,
178  HDL32 = 2,
179  HDL64 = 3
180  };
181 
182  /** LIDAR return type */
184  {
189  };
190 
191  /** Hard-wired properties of LIDARs depending on the model */
193  {
194  double maxRange;
195  };
196  using model_properties_list_t = std::map<model_t, TModelProperties>;
197  /** Access to default sets of parameters for Velodyne LIDARs */
199  {
200  /** Singleton access */
201  static const model_properties_list_t& get();
202  /** Return human-readable string: "`VLP16`,`XXX`,..." */
203  static std::string getListKnownModels();
204  };
205 
206  protected:
208  /** Default: "VLP16" */
210  /** Default: 0.5 seconds */
212  /** Default: 30 seconds */
214  /** Default: "" (no IP-based filtering) */
215  std::string m_device_ip;
216  /** Default: true Output whole frames and not data packets */
217  bool m_return_frames{true};
218  /** Default: true Output PCAP Info msgs */
219  bool m_pcap_verbose{true};
220  /** Default: "" (do not operate from an offline file) */
221  std::string m_pcap_input_file;
222  /** Default: "" (do not dump to an offline file) */
223  std::string m_pcap_output_file;
225  /** Device calibration file (supplied by vendor in an XML file) */
228 
229  // offline operation:
230  /** opaque ptr: "pcap_t*" */
231  void* m_pcap{nullptr};
232  /** opaque ptr: "pcap_t*" */
233  void* m_pcap_out{nullptr};
234  /** opaque ptr: "pcap_dumper_t *" */
235  void* m_pcap_dumper{nullptr};
236  /** opaque ptr: bpf_program* */
237  void* m_pcap_bpf_program{nullptr};
238  bool m_pcap_file_empty{true};
239  /** number of pkts read from the file so far (for debugging) */
240  unsigned int m_pcap_read_count;
241  /** Default: false */
242  bool m_pcap_read_once{false};
243  /** (Default: false) If false, will use m_pcap_read_full_scan_delay_ms */
244  bool m_pcap_read_fast{false};
245  /** (Default:100 ms) delay after each full scan read from a PCAP log */
247  /** Default: 0 (in seconds) */
248  double m_pcap_repeat_delay{0.0};
249 
250  /** See the class documentation at the top for expected parameters */
252  const mrpt::config::CConfigFileBase& configSource,
253  const std::string& section) override;
254 
255  public:
257  ~CVelodyneScanner() override;
258 
259  /** @name Change configuration parameters; to be called BEFORE initialize();
260  * see above for the list of parameters and their meaning
261  * @{ */
262  /** See supported model names in the general discussion docs for
263  * mrpt::hwdrivers::CVelodyneScanner */
264  void setModelName(const model_t model) { m_model = model; }
265  model_t getModelName() const { return m_model; }
266  /** Set the minimum period between the generation of
267  * mrpt::obs::CObservationGPS observations from Velodyne Position RMC GPS
268  * packets */
269  void setPosPacketsMinPeriod(double period_seconds)
270  {
271  m_pos_packets_min_period = period_seconds;
272  }
274  /** Set how long to wait, after loss of GPS signal, to report timestamps as
275  * "not based on satellite time". 30 secs, with typical velodyne clock
276  * drifts, means a ~1.7 ms typical drift. */
277  void setPosPacketsTimingTimeout(double timeout)
278  {
280  }
282  {
284  }
285 
286  /** UDP packets from other IPs will be ignored. Default: empty string, means
287  * do not filter by IP */
288  void setDeviceIP(const std::string& ip) { m_device_ip = ip; }
289  const std::string& getDeviceIP() const { return m_device_ip; }
290  /** Enables/disables PCAP info messages to console (default: true) */
292  /** Enables reading from a PCAP file instead of live UDP packet listening */
293  void setPCAPInputFile(const std::string& pcap_file)
294  {
295  m_pcap_input_file = pcap_file;
296  }
297  const std::string& getPCAPInputFile() const { return m_pcap_input_file; }
298  /** Enables dumping to a PCAP file in parallel to returning regular MRPT
299  * objects. Default="": no pcap log. */
300  void setPCAPOutputFile(const std::string& out_pcap_file)
301  {
302  m_pcap_output_file = out_pcap_file;
303  }
304  const std::string& getPCAPOutputFile() const { return m_pcap_output_file; }
305  void setPCAPInputFileReadOnce(bool read_once)
306  {
307  m_pcap_read_once = read_once;
308  }
311  {
312  return m_velodyne_calib;
313  }
315  {
316  m_velodyne_calib = calib;
317  }
318  /** Returns false on error. \sa
319  * mrpt::obs::VelodyneCalibration::loadFromXMLFile() */
320  bool loadCalibrationFile(const std::string& velodyne_xml_calib_file_path);
321 
322  /** Changes among STRONGEST, LAST, DUAL return types (via HTTP post
323  * interface).
324  * Can be called at any instant, before or after initialize().
325  * Requires setting a device IP address.
326  * \return false on error */
327  bool setLidarReturnType(return_type_t ret_type);
328 
329  /** Changes Lidar RPM (valid range: 300-600) (via HTTP post interface).
330  * Can be called at any instant, before or after initialize().
331  * Requires setting a device IP address.
332  * \return false on error*/
333  bool setLidarRPM(int rpm);
334 
335  /** Switches the LASER on/off (saves energy when not measuring) (via HTTP
336  * post interface).
337  * Can be called at any instant, before or after initialize().
338  * Requires setting a device IP address.
339  * \return false on error*/
340  bool setLidarOnOff(bool on);
341 
342  /** Switches whole frame (points in a single revolution) on/off publication
343  * to data packet publication. When on, getNextObservation() will return
344  * true whenever a frame is avaliable, when off, getNextObservation() will
345  * return true whenever a data packet is avaliable. The default is on. When
346  * listening to data packets on a PCAP, pcap_read_fast is enforced.
347  */
348  void setFramePublishing(bool on);
349 
350  /** @} */
351 
352  /** Polls the UDP port for incoming data packets. The user *must* call this
353  * method in a timely fashion to grab data as it it generated by the device.
354  * The minimum call rate should be the expected number of data
355  * packets/second (!=scans/second). Checkout Velodyne user manual if in
356  * doubt.
357  *
358  * \param[out] outScan Upon return, an empty smart pointer will be found
359  * here if no new data was available. Otherwise, a valid scan.
360  * \param[out] outGPS Upon return, an empty smart pointer will be found
361  * here if no new GPS data was available. Otherwise, a valid GPS reading.
362  * \return true if no error ocurred (even if there was no new observation).
363  * false if any communication error occurred.
364  */
365  bool getNextObservation(
368 
369  // See docs in parent class
370  void doProcess() override;
371 
372  /** Tries to initialize the sensor driver, after setting all the parameters
373  * with a call to loadConfig.
374  * Velodyne specifics: this method sets up the UDP listening sockets, so
375  * all relevant params MUST BE SET BEFORE calling this.
376  * \exception This method must throw an exception with a descriptive
377  * message if some critical error is found.
378  */
379  void initialize() override;
380 
381  /** Close the UDP sockets set-up in \a initialize(). This is called
382  * automatically upon destruction */
383  void close();
384 
385  /** Users normally would prefer calling \a getNextObservation() instead.
386  * This method polls the UDP data port and returns one Velodyne DATA packet
387  * (1206 bytes) and/or one POSITION packet. Refer to Velodyne users manual.
388  * Approximate timestamps (based on this computer clock) are returned for
389  * each kind of packets, or INVALID_TIMESTAMP if timeout ocurred waiting for
390  * a packet.
391  * \return true on all ok. false only for pcap reading EOF
392  */
393  bool receivePackets(
394  mrpt::system::TTimeStamp& data_pkt_timestamp,
396  mrpt::system::TTimeStamp& pos_pkt_timestamp,
398  out_pos_pkt);
399 
400  private:
401  /** Handles for the UDP sockets, or INVALID_SOCKET (-1) */
402  using platform_socket_t =
403 #ifdef _WIN32
404 #if MRPT_WORD_SIZE == 64
405  uint64_t
406 #else
407  uint32_t
408 #endif
409 #else
410  int
411 #endif
412  ;
413 
415 
417  platform_socket_t hSocket, uint8_t* out_buffer,
418  const size_t expected_packet_size,
419  const std::string& filter_only_from_IP);
420 
422  mrpt::system::TTimeStamp& data_pkt_time, uint8_t* out_data_buffer,
423  mrpt::system::TTimeStamp& pos_pkt_time, uint8_t* out_pos_buffer);
424 
425  /** In progress RX scan */
427 
430  int m_lidar_rpm{0};
432 
433  bool internal_send_http_post(const std::string& post_data);
434 
435 }; // end of class
436 } // namespace mrpt::hwdrivers
438 using namespace mrpt::hwdrivers;
443 
444 MRPT_ENUM_TYPE_BEGIN(mrpt::hwdrivers::CVelodyneScanner::return_type_t)
445 using namespace mrpt::hwdrivers;
A generic interface for a wide-variety of sensors designed to be used in the application RawLogGrabbe...
Velodyne calibration data, for usage in mrpt::obs::CObservationVelodyneScan.
A C++ interface to Velodyne laser scanners (HDL-64, HDL-32, VLP-16), working on Linux and Windows...
static short int VELODYNE_POSITION_UDP_PORT
Default: 8308.
void setPosPacketsMinPeriod(double period_seconds)
Set the minimum period between the generation of mrpt::obs::CObservationGPS observations from Velodyn...
std::map< model_t, TModelProperties > model_properties_list_t
void setPCAPInputFile(const std::string &pcap_file)
Enables reading from a PCAP file instead of live UDP packet listening.
void setPCAPVerbosity(const bool verbose)
Enables/disables PCAP info messages to console (default: true)
const std::string & getDeviceIP() const
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)
bool m_return_frames
Default: true Output whole frames and not data packets.
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.
Contains classes for various device interfaces.
void * m_pcap_bpf_program
opaque ptr: bpf_program*
void setModelName(const model_t model)
See supported model names in the general discussion docs for mrpt::hwdrivers::CVelodyneScanner.
bool m_pcap_read_fast
(Default: false) If false, will use m_pcap_read_full_scan_delay_ms
mrpt::system::TTimeStamp m_last_pos_packet_timestamp
double m_pcap_repeat_delay
Default: 0 (in seconds)
void setPosPacketsTimingTimeout(double timeout)
Set how long to wait, after loss of GPS signal, to report timestamps as "not based on satellite time"...
mrpt::obs::VelodyneCalibration m_velodyne_calib
Device calibration file (supplied by vendor in an XML file)
void setPCAPOutputFile(const std::string &out_pcap_file)
Enables dumping to a PCAP file in parallel to returning regular MRPT objects.
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
const std::string & getPCAPOutputFile() const
const std::string & getPCAPInputFile() const
This class allows loading and storing values and vectors of different types from a configuration text...
const mrpt::obs::VelodyneCalibration & getCalibration() const
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).
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.
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)
Hard-wired properties of LIDARs depending on the model.
bool m_pcap_verbose
Default: true Output PCAP Info msgs.
void setCalibration(const mrpt::obs::VelodyneCalibration &calib)
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
bool setLidarReturnType(return_type_t ret_type)
Changes among STRONGEST, LAST, DUAL return types (via HTTP post interface).
std::string m_pcap_input_file
Default: "" (do not operate from an offline file)
MRPT_FILL_ENUM_MEMBER(CVelodyneScanner, VLP16)
void setFramePublishing(bool on)
Switches whole frame (points in a single revolution) on/off publication to data packet publication...
void initialize() override
Tries to initialize the sensor driver, after setting all the parameters with a call to loadConfig...
#define DEFINE_GENERIC_SENSOR(class_name)
This declaration must be inserted in all CGenericSensor classes definition, within the class declarat...
void * m_pcap_out
opaque ptr: "pcap_t*"
bool internal_send_http_post(const std::string &post_data)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double m_pos_packets_timing_timeout
Default: 30 seconds.
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)
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*"
void setPCAPInputFileReadOnce(bool read_once)
uint32_t platform_socket_t
Handles for the UDP sockets, or INVALID_SOCKET (-1)
Access to default sets of parameters for Velodyne LIDARs.
void * m_pcap_dumper
opaque ptr: "pcap_dumper_t *"
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 MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
bool loadCalibrationFile(const std::string &velodyne_xml_calib_file_path)
Returns false on error.
mrpt::system::TTimeStamp m_last_gps_rmc_age
mrpt::obs::CObservationVelodyneScan::Ptr m_rx_scan
In progress RX scan.
void setDeviceIP(const std::string &ip)
UDP packets from other IPs will be ignored.
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.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020