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

#include <lslidar_parser.h>

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

Public 成员函数

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

构造及析构函数说明

◆ Lslidar32Parser()

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

在文件 lslidar32_parser.cc24 行定义.

24 :
25 LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) {
26 adjust_angle = 0;
27 adjust_angle_two = 0;
28 adjust_angle_three = 0;
29 adjust_angle_four = 0;
30 read_difop_ = true;
31 config_vert_angle = false;
32 lslidar_type = 2;
33}

◆ ~Lslidar32Parser()

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

在文件 lslidar_parser.h590 行定义.

590{}

成员函数说明

◆ GeneratePointcloud()

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

在文件 lslidar32_parser.cc35 行定义.

37 {
38 // allocate a point cloud with same time and frame ID as raw data
39 out_msg->mutable_header()->set_timestamp_sec(
40 scan_msg->basetime() / 1000000000.0);
41 out_msg->mutable_header()->set_module_name(
42 scan_msg->header().module_name());
43 out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id());
44 out_msg->set_height(1);
45 out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0);
46 out_msg->mutable_header()->set_sequence_num(
47 scan_msg->header().sequence_num());
48 gps_base_usec_ = scan_msg->basetime();
49
50 double scan_altitude_original_degree33[32];
51 double scan_altitude_original_degree1[32];
52 int startpos = 0;
53
54 const unsigned char* difop_ptr
55 = (const unsigned char*)scan_msg->difop_pkts(0).data().c_str();
56 if (difop_ptr[0] == 0xa5 && difop_ptr[1] == 0xff && difop_ptr[2] == 0x00
57 && difop_ptr[3] == 0x5a) {
58 if ((difop_ptr[1202] >= 0x02 && difop_ptr[1203] >= 0x80)
59 || difop_ptr[1202] == 0x03) {
60 lslidar_type = 3;
61 startpos = 245;
62 // Horizontal correction Angle
63 adjust_angle
64 = (difop_ptr[186] * 256
65 + difop_ptr[187]); // Angle correction A1
66 if (adjust_angle > 32767) {
67 adjust_angle = adjust_angle - 65535;
68 }
69 adjust_angle_two
70 = (difop_ptr[190] * 256
71 + difop_ptr[191]); // Angle correction A2
72 if (adjust_angle_two > 32767) {
73 adjust_angle_two = adjust_angle_two - 65535;
74 }
75 adjust_angle_three
76 = (difop_ptr[188] * 256
77 + difop_ptr[189]); // Angle correction A3
78 if (adjust_angle_three > 32767) {
79 adjust_angle_three = adjust_angle_three - 65535;
80 }
81 adjust_angle_four
82 = (difop_ptr[192] * 256
83 + difop_ptr[193]); // Angle correction A4
84 if (adjust_angle_four > 32767) {
85 adjust_angle_four = adjust_angle_four - 65535;
86 }
87 memcpy(scan_altitude_original_degree1,
88 scan_altitude_original_A3,
89 32 * 8);
90 memcpy(scan_altitude_original_degree33,
91 scan_altitude_original_C3,
92 32 * 8);
93
94 if (difop_ptr[185] == 0 || difop_ptr[185] == 1) {
95 int return_mode = difop_ptr[185] + 1;
96 config_.set_return_mode(return_mode);
97
98 if (difop_ptr[1195] == 0x21)
99 config_.set_degree_mode(2);
100 else
101 config_.set_degree_mode(1);
102
103 config_.set_distance_unit(0.4f);
104
105 for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) {
106 // 均匀1度校准两列
107 if (1 == config_.degree_mode()) {
108 cos_scan_altitude_caliration[i] = std::cos(
109 static_cast<float>(
110 scan_altitude_original_A3[i] * M_PI)
111 / 180.0f);
112 sin_scan_altitude_caliration[i] = std::sin(
113 static_cast<float>(
114 scan_altitude_original_A3[i] * M_PI)
115 / 180.0f);
116 scan_altitude_A[i] = scan_altitude_original_A3[i];
117 }
118 // 0.33度校准四列
119 if (2 == config_.degree_mode()) {
120 cos_scan_altitude_caliration[i] = std::cos(
121 static_cast<float>(
122 scan_altitude_original_C3[i] * M_PI)
123 / 180.0f);
124 sin_scan_altitude_caliration[i] = std::sin(
125 static_cast<float>(
126 scan_altitude_original_C3[i] * M_PI)
127 / 180.0f);
128 scan_altitude_C[i] = scan_altitude_original_C3[i];
129 }
130 }
131 }
132 } else {
133 lslidar_type = 2;
134 startpos = 882;
135 // Horizontal correction Angle
136 adjust_angle
137 = (difop_ptr[34] * 256
138 + difop_ptr[35]); /*Angle correction A1*/
139 if (adjust_angle > 32767) {
140 adjust_angle = adjust_angle - 65535;
141 }
142 adjust_angle_two
143 = (difop_ptr[42] * 256
144 + difop_ptr[43]); /*Angle correction A2*/
145 if (adjust_angle_two > 32767) {
146 adjust_angle_two = adjust_angle_two - 65535;
147 }
148 adjust_angle_three
149 = (difop_ptr[66] * 256
150 + difop_ptr[67]); /*Angle correction A3*/
151 if (adjust_angle_three > 32767) {
152 adjust_angle_three = adjust_angle_three - 65535;
153 }
154 adjust_angle_four
155 = (difop_ptr[68] * 256
156 + difop_ptr[69]); /*Angle correction A4*/
157 if (adjust_angle_four > 32767) {
158 adjust_angle_four = adjust_angle_four - 65535;
159 }
160 AWARN << "Load config failed, config file";
161 // Vertical Angle Calibration for device package
162 for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) {
163 // 均匀1度校准两列
164 if (1 == config_.degree_mode()) {
165 cos_scan_altitude_caliration[i] = std::cos(
166 static_cast<float>(
167 scan_altitude_original_A[i] * M_PI)
168 / 180.0f);
169 sin_scan_altitude_caliration[i] = std::sin(
170 static_cast<float>(
171 scan_altitude_original_A[i] * M_PI)
172 / 180.0f);
173 scan_altitude_A[i] = scan_altitude_original_A[i];
174 }
175 // 0.33度校准四列
176 if (2 == config_.degree_mode()) {
177 cos_scan_altitude_caliration[i] = std::cos(
178 static_cast<float>(
179 scan_altitude_original_C[i] * M_PI)
180 / 180.0f);
181 sin_scan_altitude_caliration[i] = std::sin(
182 static_cast<float>(
183 scan_altitude_original_C[i] * M_PI)
184 / 180.0f);
185 scan_altitude_C[i] = scan_altitude_original_C[i];
186 }
187 }
188 memcpy(scan_altitude_original_degree1,
189 scan_altitude_original_A,
190 32 * 8);
191 memcpy(scan_altitude_original_degree33,
192 scan_altitude_original_C,
193 32 * 8);
194 }
195 }
196
197 // Vertical Angle parse
198 if (config_.config_vert()) {
199 for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) {
200 uint8_t data1 = difop_ptr[startpos + 2 * i];
201 uint8_t data2 = difop_ptr[startpos + 2 * i + 1];
202 int vert_angle = data1 * 256 + data2;
203 if (vert_angle > 32767) {
204 vert_angle = vert_angle - 65535;
205 }
206 // 均匀1度校准两列
207 if (1 == config_.degree_mode()) {
208 scan_altitude_A[i]
209 = static_cast<double>(vert_angle * ROTATION_RESOLUTION);
210 if (fabs(scan_altitude_original_degree1[i] - scan_altitude_A[i])
211 > 1.5) {
212 scan_altitude_A[i] = scan_altitude_original_degree1[i];
213 }
214 config_vert_angle = true;
215 }
216 // 0.33度校准四列
217 if (2 == config_.degree_mode()) {
218 scan_altitude_C[i]
219 = static_cast<double>(vert_angle * ROTATION_RESOLUTION);
220 if (fabs(scan_altitude_original_degree33[i]
221 - scan_altitude_C[i])
222 > 1.5) {
223 scan_altitude_C[i] = scan_altitude_original_degree33[i];
224 }
225 config_vert_angle = true;
226 }
227 }
228 }
229
230 size_t packets_size = scan_msg->firing_pkts_size();
231 block_num = 0;
232 packet_number_ = packets_size;
233
234 AINFO << "packets_size :" << packets_size;
235
236 for (size_t i = 0; i < packets_size; ++i) {
237 Unpack(scan_msg->firing_pkts(static_cast<int>(i)), out_msg, i);
238 last_time_stamp_ = out_msg->measurement_time();
239 ADEBUG << "stamp: " << std::fixed << last_time_stamp_;
240 }
241
242 if (out_msg->point().empty()) {
243 // we discard this pointcloud if empty
244 AERROR << "All points is NAN!Please check lslidar:" << config_.model();
245 }
246
247 // set default width
248 out_msg->set_width(out_msg->point_size());
249}
double cos_scan_altitude_caliration[LSC32_SCANS_PER_FIRING]
double sin_scan_altitude_caliration[LSC32_SCANS_PER_FIRING]
#define ADEBUG
Definition log.h:41
#define AERROR
Definition log.h:44
#define AINFO
Definition log.h:42
#define AWARN
Definition log.h:43

◆ Order()

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

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

在文件 lslidar32_parser.cc601 行定义.

601 {
602 int width = 32;
603 cloud->set_width(width);
604 int height = cloud->point_size() / cloud->width();
605 cloud->set_height(height);
606
607 std::shared_ptr<PointCloud> cloud_origin = std::make_shared<PointCloud>();
608 cloud_origin->CopyFrom(*cloud);
609
610 for (int i = 0; i < width; ++i) {
611 int col = ORDER_32[i];
612
613 for (int j = 0; j < height; ++j) {
614 // make sure offset is initialized, should be init at setup() just
615 // once
616 int target_index = j * width + i;
617 int origin_index = j * width + col;
618 cloud->mutable_point(target_index)
619 ->CopyFrom(cloud_origin->point(origin_index));
620 }
621 }
622}
uint32_t height
Height of point cloud
uint32_t width
Width of point cloud

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