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

Lslidar data conversion class 更多...

#include <lslidar_parser.h>

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

Public 成员函数

 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 成员函数

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 属性

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 data conversion class

在文件 lslidar_parser.h496 行定义.

构造及析构函数说明

◆ LslidarParser() [1/2]

apollo::drivers::lslidar::parser::LslidarParser::LslidarParser ( )
inline

在文件 lslidar_parser.h498 行定义.

498{}

◆ LslidarParser() [2/2]

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

在文件 lslidar_parser.cc24 行定义.

24 :
25 last_time_stamp_(0), config_(config) {
26 for (int i = 0; i < 4; ++i) {
27 prism_angle64[i] = i * 0.35;
28 }
29 for (int j = 0; j < 128; ++j) {
30 // 右边
31 if (j / 4 % 2 == 0) {
32 theat1_s[j] = sin((-25 + floor(j / 8) * 2.5) * M_PI / 180);
33 theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180);
34 theat1_c[j] = cos((-25 + floor(j / 8) * 2.5) * M_PI / 180);
35 theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180);
36 } else { // 左边
37 theat1_s[j] = sin((-24 + floor(j / 8) * 2.5) * M_PI / 180);
38 theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180);
39 theat1_c[j] = cos((-24 + floor(j / 8) * 2.5) * M_PI / 180);
40 theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180);
41 }
42 }
43}
float cos(Angle16 a)
Definition angle.cc:42
float sin(Angle16 a)
Definition angle.cc:25

◆ ~LslidarParser()

virtual apollo::drivers::lslidar::parser::LslidarParser::~LslidarParser ( )
inlinevirtual

在文件 lslidar_parser.h500 行定义.

500{}

成员函数说明

◆ ComputeCoords()

void apollo::drivers::lslidar::parser::LslidarParser::ComputeCoords ( const float &  raw_distance,
LaserCorrection corrections,
const uint16_t  rotation,
PointXYZIT point 
)
protected

Use standard ROS coordinate system (right-hand rule)

在文件 lslidar_parser.cc71 行定义.

75 {
76 double x = 0.0;
77 double y = 0.0;
78 double z = 0.0;
79 double distance_corr_x = 0;
80 double distance_corr_y = 0;
81
82 corrections->cos_rot_correction = cosf(corrections->rot_correction);
83 corrections->sin_rot_correction = sinf(corrections->rot_correction);
84 corrections->cos_vert_correction = cosf(corrections->vert_correction);
85 corrections->sin_vert_correction = sinf(corrections->vert_correction);
86
87 double distance = raw_distance + corrections->dist_correction;
88
89 double cos_rot_angle
90 = cos_azimuth_table[rotation] * corrections->cos_rot_correction
91 + sin_azimuth_table[rotation] * corrections->sin_rot_correction;
92 double sin_rot_angle
93 = sin_azimuth_table[rotation] * corrections->cos_rot_correction
94 - cos_azimuth_table[rotation] * corrections->sin_rot_correction;
95
96 // Compute the distance in the xy plane (w/o accounting for rotation)
97 double xy_distance = distance * corrections->cos_vert_correction;
98
99 // Get 2points calibration values,Linear interpolation to get distance
100 // correction for X and Y, that means distance correction use
101 // different value at different distance
102 distance_corr_x = distance_corr_y = corrections->dist_correction;
103
104 double distance_x = raw_distance + distance_corr_x;
105 xy_distance = distance_x * corrections->cos_vert_correction;
106
107 x = xy_distance * sin_rot_angle
108 - corrections->horiz_offset_correction * cos_rot_angle;
109
110 double distance_y = raw_distance + distance_corr_y;
111 xy_distance = distance_y * corrections->cos_vert_correction;
112
113 y = xy_distance * cos_rot_angle
114 + corrections->horiz_offset_correction * sin_rot_angle;
115 z = distance * corrections->sin_vert_correction
116 + corrections->vert_offset_correction;
117
119 point->set_x(static_cast<float>(-y));
120 point->set_y(static_cast<float>(-x));
121 point->set_z(static_cast<float>(z));
122}

◆ ComputeCoords2()

void apollo::drivers::lslidar::parser::LslidarParser::ComputeCoords2 ( int  Laser_ring,
int  Type,
const float &  raw_distance,
LaserCorrection corrections,
const double  rotation,
PointXYZIT point 
)
protected

Use standard ROS coordinate system (right-hand rule)

在文件 lslidar_parser.cc124 行定义.

130 {
131 double x = 0.0;
132 double y = 0.0;
133 double z = 0.0;
134 double distance_corr_x = 0;
135 double distance_corr_y = 0;
136 double sinTheta_1[128] = {0};
137 double cosTheta_1[128] = {0};
138 double sinTheta_2[128] = {0};
139 double cosTheta_2[128] = {0};
140 double cos_xita;
141 // 垂直角度
142 double sin_theat;
143 double cos_theat;
144 double _R_;
145 // 水平角度
146 double cos_H_xita;
147 double sin_H_xita;
148 double cos_xita_F;
149
150 for (int i = 0; i < 128; i++) {
151 sinTheta_1[i] = sin(big_angle[i / 4] * M_PI / 180);
152 cosTheta_1[i] = cos(big_angle[i / 4] * M_PI / 180);
153
154 if (abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6
155 && abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6) {
156 sinTheta_2[i] = sin((i % 4) * (-0.17) * M_PI / 180);
157 cosTheta_2[i] = cos((i % 4) * (-0.17) * M_PI / 180);
158 } else {
159 sinTheta_2[i] = sin(prism_angle[i % 4] * M_PI / 180);
160 cosTheta_2[i] = cos(prism_angle[i % 4] * M_PI / 180);
161 }
162 }
163
164 corrections->cos_rot_correction = cosf(corrections->rot_correction);
165 corrections->sin_rot_correction = sinf(corrections->rot_correction);
166
167 double cos_azimuth_half = cos(rotation * 0.5);
168
169 if (Type == CH16) {
170 if (abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6
171 && abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6) {
172 corrections->sin_vert_correction
173 = sin_scan_laser_altitude[Laser_ring / 4 + 2]
174 + 2 * cos_azimuth_half
175 * sin_scan_mirror_altitude[Laser_ring % 4];
176 } else {
177 corrections->sin_vert_correction
178 = sin_scan_laser_altitude[Laser_ring / 4 + 2]
179 + 2 * cos_azimuth_half
180 * sin(prism_angle[Laser_ring % 4] * M_PI / 180);
181 }
182 } else if (Type == CH32) {
183 corrections->sin_vert_correction
184 = sin_scan_laser_altitude[Laser_ring / 4]
185 + 2 * cos_azimuth_half
186 * sin_scan_mirror_altitude[Laser_ring % 4];
187 } else if (Type == CH64) {
188 if (Laser_ring % 8 == 0 || Laser_ring % 8 == 1 || Laser_ring % 8 == 2
189 || Laser_ring % 8 == 3) {
190 corrections->sin_vert_correction
191 = sin(-13.33 * DEG_TO_RAD
192 + floor(Laser_ring / 4) * 1.33 * DEG_TO_RAD)
193 + 2 * cos(rotation / 2 + 1.05 * DEG_TO_RAD)
194 * sin((Laser_ring % 4) * 0.33 * DEG_TO_RAD);
195 } else if (
196 Laser_ring % 8 == 4 || Laser_ring % 8 == 5
197 || Laser_ring % 8 == 6 || Laser_ring % 8 == 7) {
198 corrections->sin_vert_correction
199 = sin(-13.33 * DEG_TO_RAD
200 + floor(Laser_ring / 4) * 1.33 * DEG_TO_RAD)
201 + 2 * cos(rotation / 2 - 1.05 * DEG_TO_RAD)
202 * sin((Laser_ring % 4) * 0.33 * DEG_TO_RAD);
203 }
204 } else if (Type == CH64w) {
205 if (Laser_ring / 4 % 2 == 0) {
206 cos_xita = cos((rotation / 2.0 + 22.5) * M_PI / 180);
207 } else {
208 cos_xita = cos((-rotation / 2.0 + 112.5) * M_PI / 180);
209 }
210 _R_ = theat2_c[Laser_ring] * theat1_c[Laser_ring] * cos_xita
211 - theat2_s[Laser_ring] * theat1_s[Laser_ring];
212 sin_theat = theat1_s[Laser_ring] + 2 * _R_ * theat2_s[Laser_ring];
213 cos_theat = sqrt(1 - pow(sin_theat, 2));
214 cos_H_xita = (2 * _R_ * theat2_c[Laser_ring] * cos_xita
215 - theat1_c[Laser_ring])
216 / cos_theat;
217 sin_H_xita = sqrt(1 - pow(cos_H_xita, 2));
218
219 if (Laser_ring / 4 % 2 == 0) {
220 cos_xita_F = (cos_H_xita + sin_H_xita) * sqrt(0.5);
221 corrections->sin_vert_correction = sqrt(1 - pow(cos_xita_F, 2));
222 } else {
223 cos_xita_F = (cos_H_xita + sin_H_xita) * (-sqrt(0.5));
224 corrections->sin_vert_correction = sqrt(1 - pow(cos_xita_F, 2));
225 }
226 } else if (Type == CH120) {
227 corrections->sin_vert_correction = sinf(corrections->vert_correction);
228 } else if (Type == CH128) {
229 if (Laser_ring / 4 % 2 == 0) {
230 cos_azimuth_half = sin((rotation - 15 * DEG_TO_RAD) * 0.5);
231 } else {
232 cos_azimuth_half = cos((rotation + 15 * DEG_TO_RAD) * 0.5);
233 }
234 corrections->sin_vert_correction
235 = sin_scan_laser_altitude_ch128[Laser_ring / 4]
236 + 2 * cos_azimuth_half * sinTheta_2[Laser_ring];
237 } else if (Type == CH128X1) {
238 double _R_ = cosTheta_2[Laser_ring] * cosTheta_1[Laser_ring]
239 * cos_azimuth_half
240 - sinTheta_2[Laser_ring] * sinTheta_1[Laser_ring];
241 corrections->sin_vert_correction
242 = sinTheta_1[Laser_ring] + 2 * _R_ * sinTheta_2[Laser_ring];
243 }
244 corrections->cos_vert_correction
245 = sqrt(1 - pow(corrections->sin_vert_correction, 2));
246
247 double distance = raw_distance + corrections->dist_correction;
248
249 double cos_rot_angle = cos(rotation) * corrections->cos_rot_correction
250 + sin(rotation) * corrections->sin_rot_correction;
251 double sin_rot_angle = sin(rotation) * corrections->cos_rot_correction
252 - cos(rotation) * corrections->sin_rot_correction;
253
254 // Compute the distance in the xy plane (w/o accounting for rotation)
255 double xy_distance = distance * corrections->cos_vert_correction;
256
257 // Get 2points calibration values,Linear interpolation to get distance
258 // correction for X and Y, that means distance correction use
259 // different value at different distance
260 distance_corr_x = distance_corr_y = corrections->dist_correction;
261
262 double distance_x = raw_distance + distance_corr_x;
263 xy_distance = distance_x * corrections->cos_vert_correction;
264
265 x = xy_distance * sin_rot_angle
266 - corrections->horiz_offset_correction * cos_rot_angle;
267
268 double distance_y = raw_distance + distance_corr_y;
269 xy_distance = distance_y * corrections->cos_vert_correction;
270
271 y = xy_distance * cos_rot_angle
272 + corrections->horiz_offset_correction * sin_rot_angle;
273 z = distance * corrections->sin_vert_correction
274 + corrections->vert_offset_correction;
275
277 point->set_x(static_cast<float>(-y));
278 point->set_y(static_cast<float>(-x));
279 point->set_z(static_cast<float>(z));
280}
#define DEG_TO_RAD
#define CH120
#define CH32
#define CH16
#define CH64
#define CH128
#define CH128X1
#define CH64w

◆ GeneratePointcloud()

virtual void apollo::drivers::lslidar::parser::LslidarParser::GeneratePointcloud ( const std::shared_ptr< LslidarScan > &  scan_msg,
const std::shared_ptr< apollo::drivers::PointCloud > &  out_msg 
)
pure 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::lslidar::parser::Lslidar401Parser, apollo::drivers::lslidar::parser::LslidarCH16Parser, apollo::drivers::lslidar::parser::LslidarCH32Parser, apollo::drivers::lslidar::parser::LslidarCH64Parser , 以及 apollo::drivers::lslidar::parser::LslidarCH64wParser 内被实现.

◆ get_last_timestamp()

const double apollo::drivers::lslidar::parser::LslidarParser::get_last_timestamp ( )
inline

在文件 lslidar_parser.h520 行定义.

520 {
521 return last_time_stamp_;
522 }

◆ is_scan_valid()

bool apollo::drivers::lslidar::parser::LslidarParser::is_scan_valid ( int  rotation,
float  distance 
)
protected

在文件 lslidar_parser.cc45 行定义.

45 {
46 // check range first
47 if (range < config_.min_range() || range > config_.max_range())
48 return false;
49 else
50 return true;
51}

◆ Order()

◆ setup()

void apollo::drivers::lslidar::parser::LslidarParser::setup ( )
virtual

Set up for on-line operation.

在文件 lslidar_parser.cc54 行定义.

54 {
55 if (config_.calibration()) {
57
59 AFATAL << "Unable to open calibration file: "
61 }
62 }
63 for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) {
64 float rotation_f = ROTATION_RESOLUTION
65 * static_cast<float>(rot_index * M_PI) / 180.0f;
66 cos_azimuth_table[rot_index] = cosf(rotation_f);
67 sin_azimuth_table[rot_index] = sinf(rotation_f);
68 }
69}
void read(const std::string &calibration_file)
#define AFATAL
Definition log.h:45

类成员变量说明

◆ calibration_

Calibration apollo::drivers::lslidar::parser::LslidarParser::calibration_
protected

在文件 lslidar_parser.h538 行定义.

◆ config_

Config apollo::drivers::lslidar::parser::LslidarParser::config_
protected

在文件 lslidar_parser.h537 行定义.

◆ config_vert_angle

bool apollo::drivers::lslidar::parser::LslidarParser::config_vert_angle
protected

在文件 lslidar_parser.h540 行定义.

◆ cos_azimuth_table

float apollo::drivers::lslidar::parser::LslidarParser::cos_azimuth_table[ROTATION_MAX_UNITS]
protected

在文件 lslidar_parser.h527 行定义.

◆ cos_scan_altitude_caliration

double apollo::drivers::lslidar::parser::LslidarParser::cos_scan_altitude_caliration[LSC32_SCANS_PER_FIRING]
protected

在文件 lslidar_parser.h528 行定义.

◆ current_packet_time

uint64_t apollo::drivers::lslidar::parser::LslidarParser::current_packet_time = 0
protected

在文件 lslidar_parser.h541 行定义.

◆ last_time_stamp_

double apollo::drivers::lslidar::parser::LslidarParser::last_time_stamp_
protected

在文件 lslidar_parser.h530 行定义.

◆ need_two_pt_correction_

bool apollo::drivers::lslidar::parser::LslidarParser::need_two_pt_correction_
protected

在文件 lslidar_parser.h539 行定义.

◆ previous_packet_time

uint64_t apollo::drivers::lslidar::parser::LslidarParser::previous_packet_time = 0
protected

在文件 lslidar_parser.h542 行定义.

◆ prism_angle

double apollo::drivers::lslidar::parser::LslidarParser::prism_angle[4]
protected

在文件 lslidar_parser.h535 行定义.

◆ prism_angle64

double apollo::drivers::lslidar::parser::LslidarParser::prism_angle64[4]
protected

在文件 lslidar_parser.h536 行定义.

◆ scan_altitude

double apollo::drivers::lslidar::parser::LslidarParser::scan_altitude[16]
protected

在文件 lslidar_parser.h525 行定义.

◆ sin_azimuth_table

float apollo::drivers::lslidar::parser::LslidarParser::sin_azimuth_table[ROTATION_MAX_UNITS]
protected

在文件 lslidar_parser.h526 行定义.

◆ sin_scan_altitude_caliration

double apollo::drivers::lslidar::parser::LslidarParser::sin_scan_altitude_caliration[LSC32_SCANS_PER_FIRING]
protected

在文件 lslidar_parser.h529 行定义.

◆ theat1_c

double apollo::drivers::lslidar::parser::LslidarParser::theat1_c[128]
protected

在文件 lslidar_parser.h533 行定义.

◆ theat1_s

double apollo::drivers::lslidar::parser::LslidarParser::theat1_s[128]
protected

在文件 lslidar_parser.h531 行定义.

◆ theat2_c

double apollo::drivers::lslidar::parser::LslidarParser::theat2_c[128]
protected

在文件 lslidar_parser.h534 行定义.

◆ theat2_s

double apollo::drivers::lslidar::parser::LslidarParser::theat2_s[128]
protected

在文件 lslidar_parser.h532 行定义.


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