Apollo 10.0
自动驾驶开放平台
lslidarLS128S2_parser.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
17#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h"
18
19namespace apollo {
20namespace drivers {
21namespace lslidar {
22namespace parser {
23static float g_fDistanceAcc = 0.1 * 0.01;
24static double cos30 = std::cos(DEG2RAD(30));
25static double sin30 = std::sin(DEG2RAD(30));
26static double sin60 = std::sin(DEG2RAD(60));
27
28LslidarLS128S2Parser::LslidarLS128S2Parser(const Config &config) :
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}
46
47void LslidarLS128S2Parser::GeneratePointcloud(
48 const std::shared_ptr<LslidarScan> &scan_msg,
49 const std::shared_ptr<PointCloud> &out_msg) {
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}
156
161void LslidarLS128S2Parser::Unpack(
162 int num,
163 const LslidarPacket &pkt,
164 std::shared_ptr<PointCloud> out_msg) {
165 struct Firing_LS128S2 lidardata {};
166 uint64_t packet_end_time;
167 const unsigned char *msop_ptr = (const unsigned char *)pkt.data().c_str();
168
169 packet_end_time = pkt.stamp();
170 current_packet_time = packet_end_time;
171 if (msop_ptr[1205] == 0x02) {
172 return_mode = 2;
173 }
174
175 if (return_mode == 1) {
176 double packet_interval_time = (current_packet_time - last_packet_time)
177 / (POINTS_PER_PACKET_SINGLE_ECHO / 8.0);
178 for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET_SINGLE_ECHO;
179 point_idx += 8) {
180 if ((msop_ptr[point_idx] == 0xff)
181 && (msop_ptr[point_idx + 1] == 0xaa)
182 && (msop_ptr[point_idx + 2] == 0xbb)
183 && (msop_ptr[point_idx + 3] == 0xcc)
184 && (msop_ptr[point_idx + 4] == 0xdd)) {
185 continue;
186 } else {
187 // Compute the time of the point
188 double point_time;
189 if (last_packet_time > 1e-6) {
190 point_time = packet_end_time
191 - packet_interval_time
192 * ((POINTS_PER_PACKET_SINGLE_ECHO
193 - point_idx)
194 / 8
195 - 1);
196 } else {
197 point_time = current_packet_time;
198 }
199
200 memset(&lidardata, 0, sizeof(lidardata));
201 // 水平角度
202 double fAngle_H
203 = msop_ptr[point_idx + 1] + (msop_ptr[point_idx] << 8);
204 if (fAngle_H > 32767) {
205 fAngle_H = (fAngle_H - 65536);
206 }
207 lidardata.azimuth = fAngle_H * 0.01;
208 // 垂直角度+通道号
209 int iTempAngle = msop_ptr[point_idx + 2];
210 int iChannelNumber = iTempAngle >> 6; // 左移六位 通道号
211 int iSymmbol = (iTempAngle >> 5) & 0x01; // 左移五位 符号位
212 double fAngle_V = 0.0;
213 if (1 == iSymmbol) { // 符号位 0：正数 1：负数
214 int iAngle_V = msop_ptr[point_idx + 3]
215 + (msop_ptr[point_idx + 2] << 8);
216
217 fAngle_V = iAngle_V | 0xc000;
218 if (fAngle_V > 32767) {
219 fAngle_V = (fAngle_V - 65536);
220 }
221 } else {
222 int iAngle_Hight = iTempAngle & 0x3f;
223 fAngle_V = msop_ptr[point_idx + 3] + (iAngle_Hight << 8);
224 }
225
226 lidardata.vertical_angle = fAngle_V * g_fAngleAcc_V;
227 lidardata.channel_number = iChannelNumber;
228 lidardata.distance
229 = ((msop_ptr[point_idx + 4] << 16)
230 + (msop_ptr[point_idx + 5] << 8)
231 + msop_ptr[point_idx + 6]);
232 lidardata.intensity = msop_ptr[point_idx + 7];
233 lidardata.time = point_time;
234 lidardata.azimuth = fAngle_H * 0.01;
235 convertCoordinate(lidardata);
236 }
237 }
238 } else {
239 double packet_interval_time = (current_packet_time - last_packet_time)
240 / (POINTS_PER_PACKET_DOUBLE_ECHO / 12.0);
241 for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET_DOUBLE_ECHO;
242 point_idx += 12) {
243 if ((msop_ptr[point_idx] == 0xff)
244 && (msop_ptr[point_idx + 1] == 0xaa)
245 && (msop_ptr[point_idx + 2] == 0xbb)
246 && (msop_ptr[point_idx + 3] == 0xcc)
247 && (msop_ptr[point_idx + 4] == 0xdd)) {
248 continue;
249 } else {
250 // Compute the time of the point
251 double point_time;
252 if (last_packet_time > 1e-6) {
253 point_time = packet_end_time
254 - packet_interval_time
255 * ((POINTS_PER_PACKET_DOUBLE_ECHO
256 - point_idx)
257 / 12
258 - 1);
259 } else {
260 point_time = current_packet_time;
261 }
262 memset(&lidardata, 0, sizeof(lidardata));
263 // 水平角度
264 double fAngle_H
265 = msop_ptr[point_idx + 1] + (msop_ptr[point_idx] << 8);
266 if (fAngle_H > 32767) {
267 fAngle_H = (fAngle_H - 65536);
268 }
269 lidardata.azimuth = fAngle_H * 0.01;
270
271 // 垂直角度+通道号
272 int iTempAngle = msop_ptr[point_idx + 2];
273 int iChannelNumber = iTempAngle >> 6; // 左移六位 通道号
274 int iSymmbol = (iTempAngle >> 5) & 0x01; // 左移五位 符号位
275 double fAngle_V = 0.0;
276 if (1 == iSymmbol) { // 符号位 0：正数 1：负数
277 int iAngle_V = msop_ptr[point_idx + 3]
278 + (msop_ptr[point_idx + 2] << 8);
279
280 fAngle_V = iAngle_V | 0xc000;
281 if (fAngle_V > 32767) {
282 fAngle_V = (fAngle_V - 65536);
283 }
284 } else {
285 int iAngle_Hight = iTempAngle & 0x3f;
286 fAngle_V = msop_ptr[point_idx + 3] + (iAngle_Hight << 8);
287 }
288
289 lidardata.vertical_angle = fAngle_V * g_fAngleAcc_V;
290 lidardata.channel_number = iChannelNumber;
291 lidardata.distance
292 = ((msop_ptr[point_idx + 4] << 16)
293 + (msop_ptr[point_idx + 5] << 8)
294 + msop_ptr[point_idx + 6]);
295 lidardata.intensity = msop_ptr[point_idx + 7];
296 lidardata.time = point_time;
297 convertCoordinate(lidardata); // 第一个点
298
299 lidardata.distance
300 = ((msop_ptr[point_idx + 8] << 16)
301 + (msop_ptr[point_idx + 9] << 8)
302 + msop_ptr[point_idx + 10]);
303 lidardata.intensity = msop_ptr[point_idx + 11];
304 lidardata.time = point_time;
305 convertCoordinate(lidardata); // 第二个点
306 }
307 }
308 last_packet_time = packet_end_time;
309 }
310}
311
312int LslidarLS128S2Parser::convertCoordinate(
313 const struct Firing_LS128S2 &lidardata) {
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}
391
392int LslidarLS128S2Parser::convertCoordinate(
393 const struct Firing_LS128S2 &lidardata,
394 std::shared_ptr<PointCloud> out_cloud) {
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}
479
480void LslidarLS128S2Parser::Order(std::shared_ptr<PointCloud> cloud) {}
481
482} // namespace parser
483} // namespace lslidar
484} // namespace drivers
485} // namespace apollo
apollo::drivers::lslidar::parser::LslidarLS128S2Parser::GeneratePointcloud
void GeneratePointcloud(const std::shared_ptr< LslidarScan > &scan_msg, const std::shared_ptr< PointCloud > &out_msg)
Definition lslidarLS128S2_parser.cc:47
apollo::drivers::lslidar::parser::LslidarLS128S2Parser::convertCoordinate
int convertCoordinate(const struct Firing_LS128S2 &lidardata)
Definition lslidarLS128S2_parser.cc:312
AERROR
#define AERROR
Definition log.h:44
AINFO
#define AINFO
Definition log.h:42
lslidar_parser.h
These classes Unpack raw Lslidar LIDAR packets into several useful formats.
DEG2RAD
#define DEG2RAD(x)
Raw Lslidar packet constants and structures.
Definition lslidar_parser.h:90
apollo
class register implement
Definition arena_queue.h:37
apollo::drivers::lslidar::parser::Firing_LS128S2
Definition lslidar_parser.h:466
apollo::drivers::lslidar::parser::Firing_LS128S2::channel_number
int channel_number
Definition lslidar_parser.h:473
apollo::drivers::lslidar::parser::Firing_LS128S2::vertical_angle
double vertical_angle
Definition lslidar_parser.h:467
apollo::drivers::lslidar::parser::Firing_LS128S2::time
double time
Definition lslidar_parser.h:472
apollo::drivers::lslidar::parser::Firing_LS128S2::intensity
float intensity
Definition lslidar_parser.h:471
apollo::drivers::lslidar::parser::Firing_LS128S2::azimuth
double azimuth
Definition lslidar_parser.h:469
apollo::drivers::lslidar::parser::Firing_LS128S2::distance
double distance
Definition lslidar_parser.h:470