60#include <boost/format.hpp>
62#include "modules/drivers/lidar/proto/velodyne.pb.h"
63#include "modules/drivers/lidar/proto/velodyne_config.pb.h"
64#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h"
93static const int RAW_SCAN_SIZE = 3;
94static const int SCANS_PER_BLOCK = 32;
95static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE);
97static const float ROTATION_RESOLUTION = 0.01f;
101static const uint16_t ROTATION_MAX_UNITS = 36001;
105static const float DISTANCE_MAX = 130.0f;
106static const float DISTANCE_RESOLUTION = 0.002f;
107static const float DISTANCE_MAX_UNITS =
108 (DISTANCE_MAX / DISTANCE_RESOLUTION + 1.0f);
111static const uint16_t UPPER_BANK = 0xeeff;
112static const uint16_t LOWER_BANK = 0xddff;
114static const float ANGULAR_RESOLUTION = 0.00300919f;
117static const int VLP16_FIRINGS_PER_BLOCK = 2;
118static const int VLP16_SCANS_PER_FIRING = 16;
119static const float VLP16_BLOCK_TDURATION = 110.592f;
120static const float VLP16_DSR_TOFFSET = 2.304f;
121static const float VLP16_FIRING_TOFFSET = 55.296f;
123static const float VLS128_CHANNEL_TDURATION = 2.304f;
124static const float VLS128_SEQ_TDURATION = 55.296f;
126static const float VLP32_DISTANCE_RESOLUTION = 0.004f;
127static const float VSL128_DISTANCE_RESOLUTION = 0.004f;
128static const float CHANNEL_TDURATION = 2.304f;
129static const float SEQ_TDURATION = 55.296f;
154static const int PACKET_SIZE = 1206;
156static const int PACKET_STATUS_SIZE = 4;
221static const float nan = std::numeric_limits<float>::signaling_NaN();
242 std::shared_ptr<PointCloud> out_msg) = 0;
243 virtual void setup();
245 virtual void Order(std::shared_ptr<PointCloud> cloud) = 0;
281 std::shared_ptr<PointCloud> pc) = 0;
283 uint64_t
GetGpsStamp(
double current_stamp,
double* previous_stamp,
284 uint64_t* gps_base_usec);
287 uint16_t laser_block_id) = 0;
296 std::shared_ptr<PointCloud> out_msg);
297 void Order(std::shared_ptr<PointCloud> cloud);
298 void setup()
override;
303 uint64_t GetTimestamp(
double base_time,
float time_offset,
304 uint16_t laser_block_id);
305 void Unpack(
const VelodynePacket& pkt, std::shared_ptr<PointCloud> pc);
308 const uint16_t raw_distance,
int intensity);
310 double previous_packet_stamp_[4];
311 uint64_t gps_base_usec_[4];
324 std::shared_ptr<PointCloud> out_msg);
325 void Order(std::shared_ptr<PointCloud> cloud);
328 uint64_t GetTimestamp(
double base_time,
float time_offset,
329 uint16_t laser_block_id);
330 void Unpack(
const VelodynePacket& pkt, std::shared_ptr<PointCloud> pc);
331 void UnpackVLP32C(
const VelodynePacket& pkt, std::shared_ptr<PointCloud> pc);
333 double previous_firing_stamp_;
334 uint64_t gps_base_usec_;
343 std::shared_ptr<PointCloud> out_msg);
344 void Order(std::shared_ptr<PointCloud> cloud);
347 uint64_t GetTimestamp(
double base_time,
float time_offset,
348 uint16_t laser_block_id);
349 void Unpack(
const VelodynePacket& pkt, std::shared_ptr<PointCloud> pc);
351 double previous_packet_stamp_;
352 uint64_t gps_base_usec_;
361 std::shared_ptr<PointCloud> out_msg);
362 void Order(std::shared_ptr<PointCloud> cloud);
365 uint64_t GetTimestamp(
double base_time,
float time_offset,
366 uint16_t laser_block_id);
367 void Unpack(
const VelodynePacket& pkt, std::shared_ptr<PointCloud> pc);
369 const uint16_t raw_distance,
int intensity);
371 double previous_packet_stamp_;
372 uint64_t gps_base_usec_;
Calibration class storing entire configuration for the Velodyne
void GeneratePointcloud(const std::shared_ptr< VelodyneScan > &scan_msg, std::shared_ptr< PointCloud > out_msg)
Set up for data processing.
void Order(std::shared_ptr< PointCloud > cloud)
void GeneratePointcloud(const std::shared_ptr< VelodyneScan > &scan_msg, std::shared_ptr< PointCloud > out_msg)
Set up for data processing.
void Order(std::shared_ptr< PointCloud > cloud)
void Order(std::shared_ptr< PointCloud > cloud)
void GeneratePointcloud(const std::shared_ptr< VelodyneScan > &scan_msg, std::shared_ptr< PointCloud > out_msg)
Set up for data processing.
void GeneratePointcloud(const std::shared_ptr< VelodyneScan > &scan_msg, std::shared_ptr< PointCloud > out_msg)
Set up for data processing.
void setup() override
Set up for on-line operation.
void Order(std::shared_ptr< PointCloud > cloud)
static VelodyneParser * CreateParser(Config config)
Velodyne data conversion class
virtual void setup()
Set up for on-line operation.
virtual void GeneratePointcloud(const std::shared_ptr< VelodyneScan > &scan_msg, std::shared_ptr< PointCloud > out_msg)=0
Set up for data processing.
const float(* inner_time_)[12][32]
const Calibration & get_calibration()
float cos_rot_table_[ROTATION_MAX_UNITS]
bool is_scan_valid(int rotation, float distance)
uint64_t GetGpsStamp(double current_stamp, double *previous_stamp, uint64_t *gps_base_usec)
virtual ~VelodyneParser()
const double get_last_timestamp()
PointXYZIT get_nan_point(uint64_t timestamp)
void init_angle_params(double view_direction, double view_width)
bool need_two_pt_correction_
virtual uint64_t GetTimestamp(double base_time, float time_offset, uint16_t laser_block_id)=0
virtual void Unpack(const VelodynePacket &pkt, std::shared_ptr< PointCloud > pc)=0
Unpack velodyne packet
void ComputeCoords(const float &raw_distance, const LaserCorrection &corrections, const uint16_t rotation, PointXYZIT *point)
Compute coords with the data in block
virtual void Order(std::shared_ptr< PointCloud > cloud)=0
float sin_rot_table_[ROTATION_MAX_UNITS]
constexpr int BLOCK_SIZE
Raw Velodyne packet constants and structures.
constexpr int BLOCKS_PER_PACKET
correction values for a single laser
uint16_t rotation
0-35999, divide by 100 to get degrees
uint8_t data[BLOCK_DATA_SIZE]
uint16_t laser_block_id
UPPER_BANK or LOWER_BANK
unsigned char status_type
unsigned int gps_timestamp
unsigned char status_value
RawBlock blocks[BLOCKS_PER_PACKET]
used for unpacking the first two data bytes in a block