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

#include <velodyne_parser.h>

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

Public 成员函数

 Velodyne64Parser (const Config &config)
 
 ~Velodyne64Parser ()
 
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 setup () override
 Set up for on-line operation.
 
- Public 成员函数 继承自 apollo::drivers::velodyne::VelodyneParser
 VelodyneParser ()
 
 VelodyneParser (const Config &config)
 
virtual ~VelodyneParser ()
 
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.h290 行定义.

构造及析构函数说明

◆ Velodyne64Parser()

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

在文件 velodyne64_parser.cc23 行定义.

24 : VelodyneParser(config) {
25 for (int i = 0; i < 4; i++) {
26 gps_base_usec_[i] = 0;
27 previous_packet_stamp_[i] = 0;
28 }
30 // init Unpack function and order function by model.
31 if (config_.model() == HDL64E_S2) {
33 is_s2_ = true;
34 } else { // 64E_S3
36 is_s2_ = false;
37 }
38
39 if (config_.mode() == LAST) {
40 mode_ = LAST;
41 } else if (config_.mode() == DUAL) {
42 mode_ = DUAL;
43 }
44}
const float INNER_TIME_64[12][32]
const float INNER_TIME_64E_S3[12][32]

◆ ~Velodyne64Parser()

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

在文件 velodyne_parser.h293 行定义.

293{}

成员函数说明

◆ GeneratePointcloud()

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

在文件 velodyne64_parser.cc149 行定义.

151 {
153 if (online_calibration_.decode(scan_msg) == -1) {
154 return;
155 }
156 calibration_ = online_calibration_.calibration();
157 if (config_.organized()) {
158 InitOffsets();
159 }
160 }
161
162 // allocate a point cloud with same time and frame ID as raw data
163 pointcloud->mutable_header()->set_frame_id(scan_msg->header().frame_id());
164 pointcloud->set_height(1);
165 pointcloud->mutable_header()->set_sequence_num(
166 scan_msg->header().sequence_num());
167
168 bool skip = false;
169 size_t packets_size = scan_msg->firing_pkts_size();
170 for (size_t i = 0; i < packets_size; ++i) {
171 if (gps_base_usec_[0] == 0) {
172 // only set one time type when call this function, so cannot break
173 SetBaseTimeFromPackets(scan_msg->firing_pkts(static_cast<int>(i)));
174 // If base time not ready then set empty_unpack true
175 skip = true;
176 } else {
177 CheckGpsStatus(scan_msg->firing_pkts(static_cast<int>(i)));
178 Unpack(scan_msg->firing_pkts(static_cast<int>(i)), pointcloud);
179 last_time_stamp_ = pointcloud->measurement_time();
180 ADEBUG << "stamp: " << std::fixed << last_time_stamp_;
181 }
182 }
183
184 if (skip) {
185 pointcloud->Clear();
186 } else {
187 int size = pointcloud->point_size();
188 if (size == 0) {
189 // we discard this pointcloud if empty
190 AERROR << "All points is NAN! Please check velodyne:" << config_.model();
191 } else {
192 uint64_t timestamp = pointcloud->point(size - 1).timestamp();
193 pointcloud->set_measurement_time(static_cast<double>(timestamp) / 1e9);
194 pointcloud->mutable_header()->set_lidar_timestamp(timestamp);
195 }
196 pointcloud->set_width(pointcloud->point_size());
197 }
198}
int decode(const std::shared_ptr< VelodyneScan > &scan_msgs)
#define ADEBUG
Definition log.h:41
#define AERROR
Definition log.h:44

◆ Order()

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

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

在文件 velodyne64_parser.cc307 行定义.

307 {
308 int height = 64;
309 cloud->set_height(height);
310 int width = cloud->point_size() / cloud->height();
311 cloud->set_width(width);
312
313 std::shared_ptr<PointCloud> cloud_origin = std::make_shared<PointCloud>();
314 cloud_origin->CopyFrom(*cloud);
315
316 for (int i = 0; i < height; ++i) {
317 int col = velodyne::ORDER_64[i];
318
319 for (int j = 0; j < width; ++j) {
320 // make sure offset is initialized, should be init at setup() just once
321 int row = (j + offsets_[i] + width) % width;
322 int target_index = j * height + i;
323 int origin_index = row * height + col;
324 cloud->mutable_point(target_index)
325 ->CopyFrom(cloud_origin->point(origin_index));
326 }
327 }
328}
uint32_t height
Height of point cloud
uint32_t width
Width of point cloud

◆ setup()

void apollo::drivers::velodyne::Velodyne64Parser::setup ( )
overridevirtual

Set up for on-line operation.

重载 apollo::drivers::velodyne::VelodyneParser .

在文件 velodyne64_parser.cc46 行定义.

46 {
49 InitOffsets();
50 }
51}
virtual void setup()
Set up for on-line operation.

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