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

#include <lslidar_parser.h>

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

Public 成员函数

 Lslidar16Parser (const Config &config)
 
 ~Lslidar16Parser ()
 
void GeneratePointcloud (const std::shared_ptr< apollo::drivers::lslidar::LslidarScan > &scan_msg, const std::shared_ptr< apollo::drivers::PointCloud > &out_msg)
 
void Order (std::shared_ptr< apollo::drivers::PointCloud > 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.
 
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.h561 行定义.

构造及析构函数说明

◆ Lslidar16Parser()

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

在文件 lslidar16_parser.cc27 行定义.

27 :
28 LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) {
30 config_vert_angle = false;
32
33 if (2 == config_.degree_mode()) {
34 for (int i = 0; i < LSC16_SCANS_PER_FIRING; i++) {
36 = std::cos(scan_altitude_original_2[i]);
38 = std::sin(scan_altitude_original_2[i]);
39 scan_altitude[i] = scan_altitude_original_2[i];
40 }
41 } else if (1 == config_.degree_mode()) {
42 for (int i = 0; i < LSC16_SCANS_PER_FIRING; i++) {
44 = std::cos(scan_altitude_original_1[i]);
46 = std::sin(scan_altitude_original_1[i]);
47 scan_altitude[i] = scan_altitude_original_1[i];
48 }
49 }
50
51 for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) {
52 float rotation = ROTATION_RESOLUTION
53 * static_cast<float>(rot_index * M_PI) / 180.0f;
54 cos_azimuth_table[rot_index] = cosf(rotation);
55 sin_azimuth_table[rot_index] = sinf(rotation);
56 }
57}
double cos_scan_altitude_caliration[LSC32_SCANS_PER_FIRING]
double sin_scan_altitude_caliration[LSC32_SCANS_PER_FIRING]

◆ ~Lslidar16Parser()

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

在文件 lslidar_parser.h564 行定义.

564{}

成员函数说明

◆ GeneratePointcloud()

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

在文件 lslidar16_parser.cc59 行定义.

61 {
62 // allocate a point cloud with same time and frame ID as raw data
63 out_msg->mutable_header()->set_timestamp_sec(
64 scan_msg->basetime() / 1000000000.0);
65 out_msg->mutable_header()->set_module_name(
66 scan_msg->header().module_name());
67 out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id());
68 out_msg->set_height(1);
69 out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0);
70 out_msg->mutable_header()->set_sequence_num(
71 scan_msg->header().sequence_num());
72 gps_base_usec_ = scan_msg->basetime();
73
74 const unsigned char *difop_ptr
75 = (const unsigned char *)scan_msg->difop_pkts(0).data().c_str();
76 uint8_t version_data = static_cast<uint8_t>(difop_ptr[1202]);
77
78 if (config_.config_vert()) {
79 if (2 == version_data) {
80 for (int i = 0; i < 16; i++) {
81 uint16_t vert_angle = static_cast<uint16_t>(
82 difop_ptr[234 + 2 * i] * 256
83 + difop_ptr[234 + 2 * i + 1]);
84
85 if (vert_angle > 32767) {
86 vert_angle = vert_angle - 65535;
87 }
88
90 = (static_cast<float>(vert_angle) / 100.f) * DEG_TO_RAD;
91 if (2 == config_.degree_mode()) {
92 if (scan_altitude[i] != 0) {
93 if (fabs(scan_altitude_original_2[i] - scan_altitude[i])
95 > 1.5) {
96 scan_altitude[i] = scan_altitude_original_2[i];
97 }
98 } else {
99 scan_altitude[i] = scan_altitude_original_2[i];
100 }
101 } else if (1 == config_.degree_mode()) {
102 if (scan_altitude[i] != 0) {
103 if (fabs(scan_altitude_original_1[i] - scan_altitude[i])
104 * RAD_TO_DEG
105 > 1.5) {
106 scan_altitude[i] = scan_altitude_original_1[i];
107 }
108 } else {
109 scan_altitude[i] = scan_altitude_original_1[i];
110 }
111 }
112 config_vert_angle = true;
113 }
114 } else {
115 for (int i = 0; i < 16; i++) {
116 uint16_t vert_angle = static_cast<uint16_t>(
117 difop_ptr[245 + 2 * i] * 256
118 + difop_ptr[245 + 2 * i + 1]);
119
120 if (vert_angle > 32767) {
121 vert_angle = vert_angle - 65535;
122 }
123
125 = (static_cast<float>(vert_angle) / 100.f) * DEG_TO_RAD;
126
127 if (2 == config_.degree_mode()) {
128 if (scan_altitude[i] != 0) {
129 if (fabs(scan_altitude_original_2[i] - scan_altitude[i])
130 * RAD_TO_DEG
131 > 1.5) {
132 scan_altitude[i] = scan_altitude_original_2[i];
133 }
134 } else {
135 scan_altitude[i] = scan_altitude_original_2[i];
136 }
137 } else if (1 == config_.degree_mode()) {
138 if (scan_altitude[i] != 0) {
139 if (fabs(scan_altitude_original_1[i] - scan_altitude[i])
140 * RAD_TO_DEG
141 > 1.5) {
142 scan_altitude[i] = scan_altitude_original_1[i];
143 }
144 } else {
145 scan_altitude[i] = scan_altitude_original_1[i];
146 }
147 }
148 config_vert_angle = true;
149 }
150 }
151 }
152
153 size_t packets_size = scan_msg->firing_pkts_size();
154 packet_number_ = packets_size;
155 block_num = 0;
156
157 for (size_t i = 0; i < packets_size; ++i) {
158 Unpack(scan_msg->firing_pkts(static_cast<int>(i)), out_msg, i);
159 last_time_stamp_ = out_msg->measurement_time();
160 ADEBUG << "***************stamp: " << std::fixed << last_time_stamp_;
161 }
162
163 if (out_msg->point().empty()) {
164 // we discard this pointcloud if empty
165 AERROR << "All points is NAN!Please check lslidar:" << config_.model();
166 }
167 // set default width
168 out_msg->set_width(out_msg->point_size());
169}
#define ADEBUG
Definition log.h:41
#define AERROR
Definition log.h:44
#define DEG_TO_RAD
#define RAD_TO_DEG

◆ Order()

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

实现了 apollo::drivers::lslidar::parser::LslidarParser.

在文件 lslidar16_parser.cc387 行定义.

387 {
388 int width = 16;
389 cloud->set_width(width);
390 int height = cloud->point_size() / cloud->width();
391 cloud->set_height(height);
392
393 std::shared_ptr<PointCloud> cloud_origin = std::make_shared<PointCloud>();
394 cloud_origin->CopyFrom(*cloud);
395
396 for (int i = 0; i < width; ++i) {
397 int col = ORDER_16[i];
398
399 for (int j = 0; j < height; ++j) {
400 // make sure offset is initialized, should be init at setup() just
401 // once
402 int target_index = j * width + i;
403 int origin_index = j * width + col;
404 cloud->mutable_point(target_index)
405 ->CopyFrom(cloud_origin->point(origin_index));
406 }
407 }
408}
uint32_t height
Height of point cloud
uint32_t width
Width of point cloud

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