Apollo 10.0
自动驾驶开放平台
apollo::drivers::lslidar::parser::LslidarLS128S2Parser类 参考

#include <lslidar_parser.h>

类 apollo::drivers::lslidar::parser::LslidarLS128S2Parser 继承关系图:
apollo::drivers::lslidar::parser::LslidarLS128S2Parser 的协作图:

Public 成员函数

 LslidarLS128S2Parser (const Config &config)
 
 ~LslidarLS128S2Parser ()
 
void GeneratePointcloud (const std::shared_ptr< LslidarScan > &scan_msg, const std::shared_ptr< PointCloud > &out_msg)
 
void Order (std::shared_ptr< PointCloud > cloud)
 
int convertCoordinate (const struct Firing_LS128S2 &lidardata)
 
int convertCoordinate (const struct Firing_LS128S2 &lidardata, std::shared_ptr< PointCloud > out_cloud)
 
- Public 成员函数 继承自 apollo::drivers::lslidar::parser::LslidarParser
 LslidarParser ()
 
 LslidarParser (const Config &config)
 
virtual ~LslidarParser ()
 
virtual void GeneratePointcloud (const std::shared_ptr< LslidarScan > &scan_msg, const std::shared_ptr< apollo::drivers::PointCloud > &out_msg)=0
 Set up for data processing.
 
virtual void setup ()
 Set up for on-line operation.
 
virtual void Order (std::shared_ptr< apollo::drivers::PointCloud > cloud)=0
 
const double get_last_timestamp ()
 

额外继承的成员函数

- Protected 成员函数 继承自 apollo::drivers::lslidar::parser::LslidarParser
bool is_scan_valid (int rotation, float distance)
 
void ComputeCoords (const float &raw_distance, LaserCorrection *corrections, const uint16_t rotation, PointXYZIT *point)
 
void ComputeCoords2 (int Laser_ring, int Type, const float &raw_distance, LaserCorrection *corrections, const double rotation, PointXYZIT *point)
 
- Protected 属性 继承自 apollo::drivers::lslidar::parser::LslidarParser
double scan_altitude [16]
 
float sin_azimuth_table [ROTATION_MAX_UNITS]
 
float cos_azimuth_table [ROTATION_MAX_UNITS]
 
double cos_scan_altitude_caliration [LSC32_SCANS_PER_FIRING]
 
double sin_scan_altitude_caliration [LSC32_SCANS_PER_FIRING]
 
double last_time_stamp_
 
double theat1_s [128]
 
double theat2_s [128]
 
double theat1_c [128]
 
double theat2_c [128]
 
double prism_angle [4]
 
double prism_angle64 [4]
 
Config config_
 
Calibration calibration_
 
bool need_two_pt_correction_
 
bool config_vert_angle
 
uint64_t current_packet_time = 0
 
uint64_t previous_packet_time = 0
 

详细描述

在文件 lslidar_parser.h887 行定义.

构造及析构函数说明

◆ LslidarLS128S2Parser()

apollo::drivers::lslidar::parser::LslidarLS128S2Parser::LslidarLS128S2Parser ( const Config config)
explicit

在文件 lslidarLS128S2_parser.cc28 行定义.

28 :
29 LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) {
30 // create the sin and cos table for different azimuth and vertical values
31 for (int j = 0; j < 36000; ++j) {
32 double angle = static_cast<double>(j) / 100.0 * M_PI / 180.0;
33 sin_table[j] = sin(angle);
34 cos_table[j] = cos(angle);
35 }
36
37 double mirror_angle[4]
38 = {0, -2, -1, -3}; // 摆镜角度 //根据通道不同偏移角度不同
39 for (int i = 0; i < 4; ++i) {
40 cos_mirror_angle[i] = cos(DEG2RAD(mirror_angle[i]));
41 sin_mirror_angle[i] = sin(DEG2RAD(mirror_angle[i]));
42 }
43 cur_pc.reset(new PointCloud());
44 pre_pc.reset(new PointCloud());
45}
#define DEG2RAD(x)
Raw Lslidar packet constants and structures.
float cos(Angle16 a)
Definition angle.cc:42
float sin(Angle16 a)
Definition angle.cc:25

◆ ~LslidarLS128S2Parser()

apollo::drivers::lslidar::parser::LslidarLS128S2Parser::~LslidarLS128S2Parser ( )
inline

在文件 lslidar_parser.h890 行定义.

890{}

成员函数说明

◆ convertCoordinate() [1/2]

int apollo::drivers::lslidar::parser::LslidarLS128S2Parser::convertCoordinate ( const struct Firing_LS128S2 lidardata)

在文件 lslidarLS128S2_parser.cc312 行定义.

313 {
314 if (lidardata.distance * g_fDistanceAcc > config_.max_range()
315 || lidardata.distance * g_fDistanceAcc < config_.min_range()) {
316 return -1;
317 }
318
319 if ((lidardata.azimuth < config_.scan_start_angle())
320 || (lidardata.azimuth > config_.scan_end_angle())) {
321 return -1;
322 }
323
324 double fAngle_H = 0.0; // 水平角度
325 double fAngle_V = 0.0; // 垂直角度
326 fAngle_H = lidardata.azimuth;
327 fAngle_V = lidardata.vertical_angle;
328
329 // 加畸变
330 double fSinV_angle = 0;
331 double fCosV_angle = 0;
332
333 // 振镜偏移角度 = 实际垂直角度 / 2 - 偏移值
334 double fGalvanometrtAngle = 0;
335 fGalvanometrtAngle = fAngle_V + 7.26;
336
337 while (fGalvanometrtAngle < 0.0) {
338 fGalvanometrtAngle += 360.0;
339 }
340 while (fAngle_H < 0.0) {
341 fAngle_H += 360.0;
342 }
343
344 int table_index_V = static_cast<int>(fGalvanometrtAngle * 100) % 36000;
345 int table_index_H = static_cast<int>(fAngle_H * 100) % 36000;
346
347 double fAngle_R0 = cos30 * cos_mirror_angle[lidardata.channel_number % 4]
348 * cos_table[table_index_V]
349 - sin_table[table_index_V]
350 * sin_mirror_angle[lidardata.channel_number % 4];
351
352 fSinV_angle = 2 * fAngle_R0 * sin_table[table_index_V]
353 + sin_mirror_angle[lidardata.channel_number % 4];
354 fCosV_angle = sqrt(1 - pow(fSinV_angle, 2));
355
356 double fSinCite = (2 * fAngle_R0 * cos_table[table_index_V] * sin30
357 - cos_mirror_angle[lidardata.channel_number % 4] * sin60)
358 / fCosV_angle;
359 double fCosCite = sqrt(1 - pow(fSinCite, 2));
360
361 double fSinCite_H = sin_table[table_index_H] * fCosCite
362 + cos_table[table_index_H] * fSinCite;
363 double fCosCite_H = cos_table[table_index_H] * fCosCite
364 - sin_table[table_index_H] * fSinCite;
365
366 double x_coord = 0.0, y_coord = 0.0, z_coord = 0.0;
367 x_coord = (lidardata.distance * fCosV_angle * fSinCite_H) * g_fDistanceAcc;
368 y_coord = (lidardata.distance * fCosV_angle * fCosCite_H) * g_fDistanceAcc;
369 z_coord = (lidardata.distance * fSinV_angle) * g_fDistanceAcc;
370
371 if ((y_coord >= config_.bottom_left_x() && y_coord <= config_.top_right_x())
372 && (-x_coord >= config_.bottom_left_y()
373 && -x_coord <= config_.top_right_y()))
374 return -1;
375
376 PointXYZIT *point1 = cur_pc->add_point();
377 point1->set_timestamp(lidardata.time);
378 point1->set_intensity(lidardata.intensity);
379 point1->set_x(y_coord);
380 point1->set_y(-x_coord);
381 point1->set_z(z_coord);
382
383 PointXYZIT *point2 = pre_pc->add_point();
384 point2->set_timestamp(lidardata.time);
385 point2->set_intensity(lidardata.intensity);
386 point2->set_x(y_coord);
387 point2->set_y(-x_coord);
388 point2->set_z(z_coord);
389 return 0;
390}

◆ convertCoordinate() [2/2]

int apollo::drivers::lslidar::parser::LslidarLS128S2Parser::convertCoordinate ( const struct Firing_LS128S2 lidardata,
std::shared_ptr< PointCloud out_cloud 
)

在文件 lslidarLS128S2_parser.cc392 行定义.

394 {
395 if (lidardata.distance * g_fDistanceAcc > config_.max_range()
396 || lidardata.distance * g_fDistanceAcc < config_.min_range()) {
397 return -1;
398 }
399
400 if ((lidardata.azimuth < config_.scan_start_angle())
401 || (lidardata.azimuth > config_.scan_end_angle())) {
402 return -1;
403 }
404
405 double fAngle_H = 0.0; // 水平角度
406 double fAngle_V = 0.0; // 垂直角度
407 fAngle_H = lidardata.azimuth;
408 fAngle_V = lidardata.vertical_angle;
409
410 // 加畸变
411 double fSinV_angle = 0;
412 double fCosV_angle = 0;
413
414 // 振镜偏移角度 = 实际垂直角度 / 2 - 偏移值
415 double fGalvanometrtAngle = 0;
416 fGalvanometrtAngle = fAngle_V + 7.26;
417
418 while (fGalvanometrtAngle < 0.0) {
419 fGalvanometrtAngle += 360.0;
420 }
421 while (fAngle_H < 0.0) {
422 fAngle_H += 360.0;
423 }
424
425 int table_index_V = static_cast<int>(fGalvanometrtAngle * 100) % 36000;
426 int table_index_H = static_cast<int>(fAngle_H * 100) % 36000;
427
428 double fAngle_R0 = cos30 * cos_mirror_angle[lidardata.channel_number % 4]
429 * cos_table[table_index_V]
430 - sin_table[table_index_V]
431 * sin_mirror_angle[lidardata.channel_number % 4];
432
433 fSinV_angle = 2 * fAngle_R0 * sin_table[table_index_V]
434 + sin_mirror_angle[lidardata.channel_number % 4];
435 fCosV_angle = sqrt(1 - pow(fSinV_angle, 2));
436
437 double fSinCite = (2 * fAngle_R0 * cos_table[table_index_V] * sin30
438 - cos_mirror_angle[lidardata.channel_number % 4] * sin60)
439 / fCosV_angle;
440 double fCosCite = sqrt(1 - pow(fSinCite, 2));
441
442 double fSinCite_H = sin_table[table_index_H] * fCosCite
443 + cos_table[table_index_H] * fSinCite;
444 double fCosCite_H = cos_table[table_index_H] * fCosCite
445 - sin_table[table_index_H] * fSinCite;
446
447 double x_coord = 0.0, y_coord = 0.0, z_coord = 0.0;
448 x_coord = (lidardata.distance * fCosV_angle * fSinCite_H) * g_fDistanceAcc;
449 y_coord = (lidardata.distance * fCosV_angle * fCosCite_H) * g_fDistanceAcc;
450 z_coord = (lidardata.distance * fSinV_angle) * g_fDistanceAcc;
451
452 if ((y_coord >= config_.bottom_left_x() && y_coord <= config_.top_right_x())
453 && (-x_coord >= config_.bottom_left_y()
454 && -x_coord <= config_.top_right_y()))
455 return -1;
456
457 PointXYZIT *point1 = cur_pc->add_point();
458 point1->set_timestamp(lidardata.time);
459 point1->set_intensity(lidardata.intensity);
460 point1->set_x(y_coord);
461 point1->set_y(-x_coord);
462 point1->set_z(z_coord);
463
464 PointXYZIT *point2 = pre_pc->add_point();
465 point2->set_timestamp(lidardata.time);
466 point2->set_intensity(lidardata.intensity);
467 point2->set_x(y_coord);
468 point2->set_y(-x_coord);
469 point2->set_z(z_coord);
470
471 PointXYZIT *point3 = out_cloud->add_point();
472 point3->set_timestamp(lidardata.time);
473 point3->set_intensity(lidardata.intensity);
474 point3->set_x(y_coord);
475 point3->set_y(-x_coord);
476 point3->set_z(z_coord);
477 return 0;
478}

◆ GeneratePointcloud()

void apollo::drivers::lslidar::parser::LslidarLS128S2Parser::GeneratePointcloud ( const std::shared_ptr< LslidarScan > &  scan_msg,
const std::shared_ptr< PointCloud > &  out_msg 
)

在文件 lslidarLS128S2_parser.cc47 行定义.

49 {
50 // allocate a point cloud with same time and frame ID as raw data
51 out_msg->mutable_header()->set_timestamp_sec(
52 scan_msg->basetime() / 1000000000.0);
53 out_msg->mutable_header()->set_module_name(
54 scan_msg->header().module_name());
55 out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id());
56 out_msg->set_height(1);
57 out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0);
58 out_msg->mutable_header()->set_sequence_num(
59 scan_msg->header().sequence_num());
60 gps_base_usec_ = scan_msg->basetime();
61
62 frame_count++;
63 const unsigned char *difop_ptr
64 = (const unsigned char *)scan_msg->difop_pkts(0).data().c_str();
65
66 if (difop_ptr[0] == 0x00 || difop_ptr[0] == 0xa5) {
67 if (difop_ptr[1] == 0xff && difop_ptr[2] == 0x00
68 && difop_ptr[3] == 0x5a) {
69 if (difop_ptr[231] == 64 || difop_ptr[231] == 65) {
70 is_add_frame_ = true;
71 }
72
73 int majorVersion = difop_ptr[1202];
74 int minorVersion1 = difop_ptr[1203] / 16;
75
76 // v1.1 :0.01 //v1.2以后 : 0.0025
77 if (1 > majorVersion || (1 == majorVersion && minorVersion1 > 1)) {
78 g_fAngleAcc_V = 0.0025;
79 } else {
80 g_fAngleAcc_V = 0.01;
81 }
82
83 float fInitAngle_V = difop_ptr[188] * 256 + difop_ptr[189];
84 if (fInitAngle_V > 32767) {
85 fInitAngle_V = fInitAngle_V - 65536;
86 }
87 this->prism_angle[0] = fInitAngle_V * g_fAngleAcc_V;
88
89 fInitAngle_V = difop_ptr[190] * 256 + difop_ptr[191];
90 if (fInitAngle_V > 32767) {
91 fInitAngle_V = fInitAngle_V - 65536;
92 }
93 this->prism_angle[1] = fInitAngle_V * g_fAngleAcc_V;
94
95 fInitAngle_V = difop_ptr[192] * 256 + difop_ptr[193];
96 if (fInitAngle_V > 32767) {
97 fInitAngle_V = fInitAngle_V - 65536;
98 }
99 this->prism_angle[2] = fInitAngle_V * g_fAngleAcc_V;
100
101 fInitAngle_V = difop_ptr[194] * 256 + difop_ptr[195];
102 if (fInitAngle_V > 32767) {
103 fInitAngle_V = fInitAngle_V - 65536;
104 }
105 this->prism_angle[3] = fInitAngle_V * g_fAngleAcc_V;
106 }
107 }
108
109 packets_size = scan_msg->firing_pkts_size();
110 packet_number_ = packets_size;
111
112 for (size_t i = 0; i < packets_size; ++i) {
113 Unpack(static_cast<int>(i),
114 scan_msg->firing_pkts(static_cast<int>(i)),
115 out_msg);
116 }
117
118 if (is_add_frame_) {
119 if (frame_count >= 2) {
120 // out_msg = std::move(cur_pc);
121 for (int j = 0; j < cur_pc->point_size(); ++j) {
122 PointXYZIT *point3 = out_msg->add_point();
123 point3->set_timestamp(cur_pc->point(j).timestamp());
124 point3->set_intensity(cur_pc->point(j).intensity());
125 point3->set_x(cur_pc->point(j).x());
126 point3->set_y(cur_pc->point(j).y());
127 point3->set_z(cur_pc->point(j).z());
128 }
129 }
130 cur_pc = pre_pc;
131 pre_pc.reset(new PointCloud());
132 } else {
133 // out_msg = cur_pc;
134 for (int j = 0; j < cur_pc->point_size(); ++j) {
135 PointXYZIT *point3 = out_msg->add_point();
136 point3->set_timestamp(cur_pc->point(j).timestamp());
137 point3->set_intensity(cur_pc->point(j).intensity());
138 point3->set_x(cur_pc->point(j).x());
139 point3->set_y(cur_pc->point(j).y());
140 point3->set_z(cur_pc->point(j).z());
141 }
142 cur_pc.reset(new PointCloud());
143 pre_pc.reset(new PointCloud());
144 }
145 AINFO << "line: " << __LINE__ << "out_msg size: " << out_msg->point_size();
146 AINFO << "packets_size :" << packets_size;
147 if (out_msg->point().empty()) {
148 // we discard this pointcloud if empty
149 AERROR << "All points is NAN!Please check lslidar:" << config_.model();
150 }
151
152 // set default width
153 out_msg->set_width(out_msg->point_size());
154 out_msg->set_height(1);
155}
#define AERROR
Definition log.h:44
#define AINFO
Definition log.h:42

◆ Order()

void apollo::drivers::lslidar::parser::LslidarLS128S2Parser::Order ( std::shared_ptr< PointCloud cloud)

在文件 lslidarLS128S2_parser.cc480 行定义.

480{}

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