class mrpt::hwdrivers::CVelodyneScanner
Overview
A C++ interface to Velodyne laser scanners (HDL-64, HDL-32, VLP-16), working on Linux and Windows.
(Using this class requires WinPCap as a run-time dependency in Windows). It can receive data from real devices via an Ethernet connection or parse a WireShark PCAP file for offline processing. The choice of online vs. offline operation is taken upon calling initialize() : if a PCAP input file has been defined, offline operation takes place and network is not listened for incomming packets.
Parsing dual return scans requires a VLP-16 with firmware version 3.0.23 or newer. While converting the scan into a point cloud in mrpt::obs::CObservationVelodyneScan you can select whether to keep the strongest, the last or both laser returns.
XML calibration files are not mandatory for VLP-16 and HDL-32, but they are for HDL-64.
Grabbing live data (as a user)
Use the application velodyne-view to visualize the LIDAR output in real-time (optionally saving to a PCAP file) or to playback a PCAP file.
Use rawlog-grabber to record a dataset in MRPT’s format together with any other set of sensors. See example config file: MRPTsharemrptconfig_filesrawlog-grabbervelodyne.ini
Grabbing live data (programmatically)
See CGenericSensor for a general overview of the sequence of methods to be called: loadConfig(), initialize(), doProcess().
Or use this class inside the application rawlog-grabber. See example config files: MRPTsharemrptconfig_filesrawlog-grabbervelodyne.ini
See the source code of the example application [MRPT]/apps/velodyne-view
(velodyne-view web page) for more details.
Playing back a PCAP file:
It is common to save Velodyne datasets as Wireshark’s PCAP files. These files can be played back with tools like bittwist, which emit all UDP packets in the PCAP log. Then, use this class to receive the packets as if they come from the real sensor.
Alternatively, if MRPT is linked against libpcap, this class can directly parse a PCAP file to simulate reading from a device offline. See method setPCAPInputFile() and config file parameter ``
To compile with PCAP support: In Debian/Ubuntu, install libpcap-dev. In Windows, install WinPCap developer packages + the regular WinPCap driver.
Configuration and usage:
Data is returned as observations of type:
mrpt::obs::CObservationVelodyneScan for one or more “data packets” (refer to Velodyne usage manual)
mrpt::obs::CObservationGPS for GPS (GPRMC) packets, if available via the synchronization interface of the device. See those classes for documentation on their fields.
Configuration includes setting the device IP (optional) and sensor model (mandatory only if a calibration file is not provided). These parameters can be set programmatically (see methods of this class), or via a configuration file with CGenericSensor::loadConfig() (see example config file section below).
About timestamps:
Each gathered observation of type mrpt::obs::CObservationVelodyneScan is populated with two timestamps, one for the local PC timestamp and, if available, another one for the GPS-stamped timestamp. Refer to the observation docs for details.
Format of parameters for loading from a .ini file
PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS: ------------------------------------------------------- [supplied_section_name] # ---- Sensor description ---- #calibration_file = PUT_HERE_FULL_PATH_TO_CALIB_FILE.xml // Optional but recommended: put here your vendor-provided calibration file model = VLP16 // Can be any of: `VLP16`, `HDL32`, `HDL64` (It is used to load default calibration file. Parameter not required if `calibration_file` is provided. #pos_packets_min_period = 0.5 // (Default=0.5 seconds) Minimum period to leave between reporting position packets. Used to decimate the large number of packets of this type. # How long to wait, after loss of GPS signal, to report timestamps as "not based on satellite time". 30 secs, with typical velodyne clock drifts, means a ~1.7 ms typical drift. #pos_packets_timing_timeout = 30 // (Default=30 seconds) # ---- Online operation ---- # IP address of the device. UDP packets from other IPs will be ignored. Leave commented or blank # if only one scanner is present (no IP filtering) #device_ip = XXX.XXX.XXX.XXX #rpm = 600 // Sensor RPM (Default: unchanged). Requires setting `device_ip` #return_type = STRONGEST // Any of: 'STRONGEST', 'LAST', 'DUAL' (Default: unchanged). Requires setting `device_ip` # ---- Offline operation ---- # If uncommented, this class will read from the PCAP instead of connecting and listeling # for online network packets. # pcap_input = PUT_FULL_PATH_TO_PCAP_LOG_FILE.pcap # pcap_read_once = false // Do not loop # pcap_read_fast = false // fast forward skipping non-velodyne packets # pcap_read_full_scan_delay_ms = 100 // Used to simulate a reasonable number of full scans / second # pcap_repeat_delay = 0.0 // seconds # ---- Save to PCAP file ---- # If uncommented, a PCAP file named `[pcap_output_prefix]_[DATE_TIME].pcap` will be # written simultaneously to the normal operation of this class. # pcap_output = velodyne_log # 3D position of the sensor on the vehicle: pose_x = 0 // 3D position (meters) pose_y = 0 pose_z = 0 pose_yaw = 0 // 3D orientation (degrees) pose_pitch = 0 pose_roll = 0
Copyright notice
Portions of this class are based on code from velodyne ROS node in https://github.com/ros-drivers/velodyne Copyright (C) 2007 Austin Robot Technology, Patrick Beeson Copyright (C) 2009, 2010 Austin Robot Technology, Jack O’Quin License: Modified BSD Software License Agreement
New in MRPT 1.4.0
#include <mrpt/hwdrivers/CVelodyneScanner.h> class CVelodyneScanner: public mrpt::hwdrivers::CGenericSensor { public: // typedefs typedef std::map<model_t, TModelProperties> model_properties_list_t; // enums enum model_t; enum return_type_t; // structs struct TModelProperties; struct TModelPropertiesFactory; // fields static short int VELODYNE_DATA_UDP_PORT = 2368; static short int VELODYNE_POSITION_UDP_PORT = 8308; // construction CVelodyneScanner(); // methods void setModelName(const model_t model); model_t getModelName() const; void setPosPacketsMinPeriod(double period_seconds); double getPosPacketsMinPeriod() const; void setPosPacketsTimingTimeout(double timeout); double getPosPacketsTimingTimeout() const; void setDeviceIP(const std::string& ip); const std::string& getDeviceIP() const; void setPCAPVerbosity(const bool verbose); void setPCAPInputFile(const std::string& pcap_file); const std::string& getPCAPInputFile() const; void setPCAPOutputFile(const std::string& out_pcap_file); const std::string& getPCAPOutputFile() const; void setPCAPInputFileReadOnce(bool read_once); bool getPCAPInputFileReadOnce() const; const mrpt::obs::VelodyneCalibration& getCalibration() const; void setCalibration(const mrpt::obs::VelodyneCalibration& calib); bool loadCalibrationFile(const std::string& velodyne_xml_calib_file_path); bool setLidarReturnType(return_type_t ret_type); bool setLidarRPM(int rpm); bool setLidarOnOff(bool on); void setFramePublishing(bool on); bool getNextObservation(mrpt::obs::CObservationVelodyneScan::Ptr& outScan, mrpt::obs::CObservationGPS::Ptr& outGPS); virtual void doProcess(); virtual void initialize(); void close(); 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); };
Inherited Members
public: // methods CGenericSensor& operator = (const CGenericSensor&); virtual void doProcess() = 0;
Fields
static short int VELODYNE_DATA_UDP_PORT = 2368
Default: 2368.
Change it if required.
static short int VELODYNE_POSITION_UDP_PORT = 8308
Default: 8308.
Change it if required.
Methods
void setModelName(const model_t model)
See supported model names in the general discussion docs for mrpt::hwdrivers::CVelodyneScanner.
void setPosPacketsMinPeriod(double period_seconds)
Set the minimum period between the generation of mrpt::obs::CObservationGPS observations from Velodyne Position RMC GPS packets.
void setPosPacketsTimingTimeout(double timeout)
Set how long to wait, after loss of GPS signal, to report timestamps as “not based on satellite time”.
30 secs, with typical velodyne clock drifts, means a ~1.7 ms typical drift.
void setDeviceIP(const std::string& ip)
UDP packets from other IPs will be ignored.
Default: empty string, means do not filter by IP
void setPCAPVerbosity(const bool verbose)
Enables/disables PCAP info messages to console (default: true)
void setPCAPInputFile(const std::string& pcap_file)
Enables reading from a PCAP file instead of live UDP packet listening.
void setPCAPOutputFile(const std::string& out_pcap_file)
Enables dumping to a PCAP file in parallel to returning regular MRPT objects.
Default=””: no pcap log.
bool loadCalibrationFile(const std::string& velodyne_xml_calib_file_path)
Returns false on error.
See also:
mrpt::obs::VelodyneCalibration::loadFromXMLFile()
bool setLidarReturnType(return_type_t ret_type)
Changes among STRONGEST, LAST, DUAL return types (via HTTP post interface).
Can be called at any instant, before or after initialize(). Requires setting a device IP address.
Returns:
false on error
bool setLidarRPM(int rpm)
Changes Lidar RPM (valid range: 300-600) (via HTTP post interface).
Can be called at any instant, before or after initialize(). Requires setting a device IP address.
Returns:
false on error
bool setLidarOnOff(bool on)
Switches the LASER on/off (saves energy when not measuring) (via HTTP post interface).
Can be called at any instant, before or after initialize(). Requires setting a device IP address.
Returns:
false on error
void setFramePublishing(bool on)
Switches whole frame (points in a single revolution) on/off publication to data packet publication.
When on, getNextObservation() will return true whenever a frame is avaliable, when off, getNextObservation() will return true whenever a data packet is avaliable. The default is on. When listening to data packets on a PCAP, pcap_read_fast is enforced.
bool getNextObservation(mrpt::obs::CObservationVelodyneScan::Ptr& outScan, mrpt::obs::CObservationGPS::Ptr& outGPS)
Polls the UDP port for incoming data packets.
The user must call this method in a timely fashion to grab data as it it generated by the device. The minimum call rate should be the expected number of data packets/second (!=scans/second). Checkout Velodyne user manual if in doubt.
Parameters:
outScan |
Upon return, an empty smart pointer will be found here if no new data was available. Otherwise, a valid scan. |
outGPS |
Upon return, an empty smart pointer will be found here if no new GPS data was available. Otherwise, a valid GPS reading. |
Returns:
true if no error ocurred (even if there was no new observation). false if any communication error occurred.
virtual void doProcess()
This method will be invoked at a minimum rate of “process_rate” (Hz)
Parameters:
This |
method must throw an exception with a descriptive message if some critical error is found. |
virtual void initialize()
Tries to initialize the sensor driver, after setting all the parameters with a call to loadConfig.
Tries to initialize the sensor, after setting all the parameters with a call to loadConfig.
Velodyne specifics: this method sets up the UDP listening sockets, so all relevant params MUST BE SET BEFORE calling this.
Parameters:
This |
method must throw an exception with a descriptive message if some critical error is found. |
This |
method must throw an exception with a descriptive message if some critical error is found. |
void close()
Close the UDP sockets set-up in initialize().
This is called automatically upon destruction
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.
This method polls the UDP data port and returns one Velodyne DATA packet (1206 bytes) and/or one POSITION packet. Refer to Velodyne users manual. Approximate timestamps (based on this computer clock) are returned for each kind of packets, or INVALID_TIMESTAMP if timeout ocurred waiting for a packet.
Returns:
true on all ok. false only for pcap reading EOF