Apollo 10.0
自动驾驶开放平台
apollo::drivers::velodyne::Velodyne32Parser类 参考

#include <velodyne_parser.h>

类 apollo::drivers::velodyne::Velodyne32Parser 继承关系图:
apollo::drivers::velodyne::Velodyne32Parser 的协作图:

Public 成员函数

 Velodyne32Parser (const Config &config)
 
 ~Velodyne32Parser ()
 
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)
 
- Public 成员函数 继承自 apollo::drivers::velodyne::VelodyneParser
 VelodyneParser ()
 
 VelodyneParser (const Config &config)
 
virtual ~VelodyneParser ()
 
virtual void setup ()
 Set up for on-line operation.
 
const Calibrationget_calibration ()
 
const double get_last_timestamp ()
 

额外继承的成员函数

- Protected 成员函数 继承自 apollo::drivers::velodyne::VelodyneParser
PointXYZIT get_nan_point (uint64_t timestamp)
 
void init_angle_params (double view_direction, double view_width)
 
void ComputeCoords (const float &raw_distance, const LaserCorrection &corrections, const uint16_t rotation, PointXYZIT *point)
 Compute coords with the data in block
 
bool is_scan_valid (int rotation, float distance)
 
uint64_t GetGpsStamp (double current_stamp, double *previous_stamp, uint64_t *gps_base_usec)
 
- Protected 属性 继承自 apollo::drivers::velodyne::VelodyneParser
const float(* inner_time_ )[12][32]
 
Calibration calibration_
 
float sin_rot_table_ [ROTATION_MAX_UNITS]
 
float cos_rot_table_ [ROTATION_MAX_UNITS]
 
double last_time_stamp_
 
Config config_
 
bool need_two_pt_correction_
 
Mode mode_
 

详细描述

在文件 velodyne_parser.h318 行定义.

构造及析构函数说明

◆ Velodyne32Parser()

apollo::drivers::velodyne::Velodyne32Parser::Velodyne32Parser ( const Config config)
explicit

◆ ~Velodyne32Parser()

apollo::drivers::velodyne::Velodyne32Parser::~Velodyne32Parser ( )
inline

在文件 velodyne_parser.h321 行定义.

321{}

成员函数说明

◆ GeneratePointcloud()

void apollo::drivers::velodyne::Velodyne32Parser::GeneratePointcloud ( const std::shared_ptr< VelodyneScan > &  scan_msg,
std::shared_ptr< PointCloud out_msg 
)
virtual

Set up for data processing.

Perform initializations needed before data processing can begin:

  • read device-specific angles calibration
参数
private_nhprivate node handle for ROS parameters
返回
0 if successful; errno value for failure

实现了 apollo::drivers::velodyne::VelodyneParser.

在文件 velodyne32_parser.cc32 行定义.

34 {
35 // allocate a point cloud with same time and frame ID as raw data
36 out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id());
37 out_msg->set_height(1);
38 out_msg->mutable_header()->set_sequence_num(
39 scan_msg->header().sequence_num());
40 gps_base_usec_ = scan_msg->basetime();
41
42 size_t packets_size = scan_msg->firing_pkts_size();
43 if (config_.model() == VLP32C) {
44 for (size_t i = 0; i < packets_size; ++i) {
45 UnpackVLP32C(scan_msg->firing_pkts(static_cast<int>(i)), out_msg);
46 }
47 } else {
48 for (size_t i = 0; i < packets_size; ++i) {
49 Unpack(scan_msg->firing_pkts(static_cast<int>(i)), out_msg);
50 }
51 }
52
53 // set measurement and lidar_timestampe
54 int size = out_msg->point_size();
55 if (size == 0) {
56 // we discard this pointcloud if empty
57 AERROR << "All points is NAN!Please check velodyne:" << config_.model();
58 } else {
59 // take the last point's timestamp as the whole frame's measurement time.
60 uint64_t timestamp = out_msg->point(size - 1).timestamp();
61 out_msg->set_measurement_time(static_cast<double>(timestamp) / 1e9);
62 out_msg->mutable_header()->set_lidar_timestamp(timestamp);
63 }
64
65 // set default width
66 out_msg->set_width(out_msg->point_size());
67
68 last_time_stamp_ = out_msg->measurement_time();
69}
#define AERROR
Definition log.h:44

◆ Order()

void apollo::drivers::velodyne::Velodyne32Parser::Order ( std::shared_ptr< PointCloud cloud)
virtual

实现了 apollo::drivers::velodyne::VelodyneParser.

在文件 velodyne32_parser.cc227 行定义.

227 {
228 if (config_.model() == VLP32C) {
229 return;
230 }
231 int width = 32;
232 cloud->set_width(width);
233 int height = cloud->point_size() / cloud->width();
234 cloud->set_height(height);
235
236 std::shared_ptr<PointCloud> cloud_origin = std::make_shared<PointCloud>();
237 cloud_origin->CopyFrom(*cloud);
238
239 for (int i = 0; i < width; ++i) {
240 int col = velodyne::ORDER_HDL32E[i];
241
242 for (int j = 0; j < height; ++j) {
243 // make sure offset is initialized, should be init at setup() just once
244 int target_index = j * width + i;
245 int origin_index = j * width + col;
246 cloud->mutable_point(target_index)
247 ->CopyFrom(cloud_origin->point(origin_index));
248 }
249 }
250}
uint32_t height
Height of point cloud
uint32_t width
Width of point cloud

该类的文档由以下文件生成: