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

#include <velodyne_parser.h>

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

Public 成员函数

 Velodyne16Parser (const Config &config)
 
 ~Velodyne16Parser ()
 
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.h337 行定义.

构造及析构函数说明

◆ Velodyne16Parser()

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

在文件 velodyne16_parser.cc23 行定义.

24 : VelodyneParser(config), previous_packet_stamp_(0), gps_base_usec_(0) {
27}
const float INNER_TIME_16[12][32]

◆ ~Velodyne16Parser()

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

在文件 velodyne_parser.h340 行定义.

340{}

成员函数说明

◆ GeneratePointcloud()

void apollo::drivers::velodyne::Velodyne16Parser::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.

在文件 velodyne16_parser.cc29 行定义.

31 {
32 // allocate a point cloud with same time and frame ID as raw data
33 out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id());
34 out_msg->set_height(1);
35 out_msg->mutable_header()->set_sequence_num(
36 scan_msg->header().sequence_num());
37 gps_base_usec_ = scan_msg->basetime();
38
39 size_t packets_size = scan_msg->firing_pkts_size();
40 for (size_t i = 0; i < packets_size; ++i) {
41 Unpack(scan_msg->firing_pkts(static_cast<int>(i)), out_msg);
42 last_time_stamp_ = out_msg->measurement_time();
43 ADEBUG << "stamp: " << std::fixed << last_time_stamp_;
44 }
45
46 if (out_msg->point().empty()) {
47 // we discard this pointcloud if empty
48 AERROR << "All points is NAN!Please check velodyne:" << config_.model();
49 }
50
51 // set default width
52 out_msg->set_width(out_msg->point_size());
53}
#define ADEBUG
Definition log.h:41
#define AERROR
Definition log.h:44

◆ Order()

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

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

在文件 velodyne16_parser.cc160 行定义.

160 {
161 int width = 16;
162 cloud->set_width(width);
163 int height = cloud->point_size() / cloud->width();
164 cloud->set_height(height);
165
166 std::shared_ptr<PointCloud> cloud_origin = std::make_shared<PointCloud>();
167 cloud_origin->CopyFrom(*cloud);
168
169 for (int i = 0; i < width; ++i) {
170 int col = velodyne::ORDER_16[i];
171
172 for (int j = 0; j < height; ++j) {
173 // make sure offset is initialized, should be init at setup() just once
174 int target_index = j * width + i;
175 int origin_index = j * width + col;
176 cloud->mutable_point(target_index)
177 ->CopyFrom(cloud_origin->point(origin_index));
178 }
179 }
180}
uint32_t height
Height of point cloud
uint32_t width
Width of point cloud
const int ORDER_16[16]
Order array for re-ordering point cloud.

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