Apollo 10.0
自动驾驶开放平台
lslidar_parser.h
浏览该文件的文档.
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/* -*- mode: C++ -*-
18 *
19 * Copyright (C) 2007 Austin Robot Technology, Yaxin Liu, Patrick Beeson
20 * Copyright (C) 2009 Austin Robot Technology, Jack O'Quin
21 *
22 * License: Modified BSD Software License Agreement
23 *
24 * $Id$
25 */
26
48#pragma once
49
50#include <cerrno>
51#include <cmath>
52#include <cstdint>
53#include <limits>
54#include <memory>
55#include <string>
56#include <thread>
57
58#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h"
59#include "modules/drivers/lidar/lslidar/proto/config.pb.h"
60#include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h"
61
62#include "cyber/cyber.h"
64
65#define DEG_TO_RAD 0.017453292
66#define RAD_TO_DEG 57.29577951
67
68#define CH16 1
69#define CH32 2
70#define CH64 3
71#define CH64w 4
72#define CH120 5
73#define CH128 6
74#define CH128X1 7
75
76namespace apollo {
77namespace drivers {
78namespace lslidar {
79namespace parser {
80
86
90#define DEG2RAD(x) ((x) * 0.017453293)
91static const int SIZE_BLOCK = 100;
92static const int RAW_SCAN_SIZE = 3;
93static const int SCANS_PER_BLOCK = 32;
94static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE);
95
96static const float ROTATION_RESOLUTION = 0.01f;
97static const uint16_t ROTATION_MAX_UNITS = 36000;
98
99static const unsigned int POINTS_ONE_CHANNEL_PER_SECOND = 20000;
100static const unsigned int BLOCKS_ONE_CHANNEL_PER_PKT = 12;
101
104static const float DISTANCE_MAX = 200.0f;
105static const float DISTANCE_MIN = 0.2f;
106static const float DISTANCE_RESOLUTION = 0.01f;
107static const double DISTANCE_RESOLUTION2 = 0.0000390625;
108static const float DISTANCE_RESOLUTION_NEW = 0.005f;
109static const float DISTANCE_MAX_UNITS
110 = (DISTANCE_MAX / DISTANCE_RESOLUTION + 1.0f);
111
112// laser_block_id
113static const uint16_t UPPER_BANK = 0xeeff;
114static const uint16_t LOWER_BANK = 0xddff;
115
117static const int LSC16_FIRINGS_PER_BLOCK = 2;
118static const int LSC16_SCANS_PER_FIRING = 16;
119static const float LSC16_BLOCK_TDURATION = 100.0f; // [µs]
120static const float LSC16_DSR_TOFFSET = 3.125f; // [µs]
121static const float LSC16_FIRING_TOFFSET = 50.0f; // [µs]
122
123static const int LSC32_FIRINGS_PER_BLOCK = 1;
124static const int LSC32_SCANS_PER_FIRING = 32;
125static const float LSC32_FIRING_TOFFSET = 100.0f; // [µs]
126static const float LSC32_AZIMUTH_TOFFSET = 12.98f;
127static const float LSC32_DISTANCE_TOFFSET = 4.94f;
129static const int SCANS_PER_FIRING_C32 = 32;
130static const int FIRING_TOFFSET_C32 = 32;
131static const double R32_1_ = 0.0361;
132static const double CX4_R1_ = 0.0361; //
133static const double CX4_C32W = 0.03416; // 新C32W byte[1211] = 0x47
134static const double CX4_R1_90 = 0.0209; // 90度
135static const double CX4_conversionAngle_ = 20.25;
136static const double CX4_conversionAngle_90 = 27.76; // 90度
137
145typedef struct raw_block {
146 uint16_t header;
147 uint8_t rotation_1;
148 uint8_t rotation_2;
150 uint8_t data[BLOCK_DATA_SIZE]; // 96
152
159 int16_t uint;
160 int8_t bytes[2];
161};
162
164 uint32_t uint;
165 uint8_t bytes[4];
166};
167static const int PACKET_SIZE = 1212;
168static const int POINTS_PER_PACKET = 171;
169static const int LS_POINTS_PER_PACKET = 149; // LS128S2 149 points
170static const int BLOCKS_PER_PACKET = 12;
171static const int PACKET_STATUS_SIZE = 4;
172static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET);
173
174const int ORDER_16[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
175const int ORDER_32[32]
176 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15,
177 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31};
178// 1,v2.6
179static const double scan_altitude_original_A[32] = {
180 -16, -15, -14, -13, -12, -11, -10, -9, -8, -7, -6, -5, -4, -3, -2, -1,
181 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
182
183// 1,v3.0
184static const double scan_altitude_original_A3[32]
185 = {-16, 0, -15, 1, -14, 2, -13, 3, -12, 4, -11, 5, -10, 6, -9, 7,
186 -8, 8, -7, 9, -6, 10, -5, 11, -4, 12, -3, 13, -2, 14, -1, 15};
187
188// 0.33,v2.6
189static const double scan_altitude_original_C[32]
190 = {-18, -15, -12, -10, -8, -7, -6, -5, -4, -3.33, -2.66,
191 -3, -2.33, -2, -1.33, -1.66, -1, -0.66, 0, -0.33, 0.33, 0.66,
192 1.33, 1, 1.66, 2, 3, 4, 6, 8, 11, 14};
193
194// 0.33,v3.0
195static const double scan_altitude_original_C3[32]
196 = {-18, -1, -15, -0.66, -12, -0.33, -10, 0, -8, 0.33, -7,
197 0.66, -6, 1, -5, 1.33, -4, 1.66, -3.33, 2, -3, 3,
198 -2.66, 4, -2.33, 6, -2, 8, -1.66, 11, -1.33, 14};
199
200// CX 4.0
201// Pre-compute the sine and cosine for the altitude angles.
202// c32 32度
203static const double c32_32_vertical_angle[32]
204 = {-16, -8, 0, 8, -15, -7, 1, 9, -14, -6, 2, 10, -13, -5, 3, 11,
205 -12, -4, 4, 12, -11, -3, 5, 13, -10, -2, 6, 14, -9, -1, 7, 15};
206// c32 70度 0x46
207static const double c32_70_vertical_angle[32]
208 = {-54, -31, -8, 2.66, -51.5, -28, -6.66, 4, -49, -25, -5.33,
209 5.5, -46, -22, -4, 7, -43, -18.5, -2.66, 9, -40, -15,
210 -1.33, 11, -37, -12, 0, 13, -34, -10, 1.33, 15};
211
212// c32 70度 0x47
213static const double c32_70_vertical_angle2[32]
214 = {-54.7, -31, -9, 3, -51.5, -28, -7.5, 4.5, -49, -25, -6.0,
215 6.0, -46, -22, -4.5, 7.5, -43, -18.5, -3.0, 9, -40, -15,
216 -1.5, 11, -37, -12, 0, 13, -34, -10.5, 1.5, 15};
217
218// c32 90度
219static const double c32_90_vertical_angle[32]
220 = {2.487, 25.174, 47.201, 68.819, 5.596, 27.811, 49.999, 71.525,
221 8.591, 30.429, 52.798, 74.274, 11.494, 33.191, 55.596, 77.074,
222 14.324, 36.008, 58.26, 79.938, 17.096, 38.808, 60.87, 82.884,
223 19.824, 41.603, 63.498, 85.933, 22.513, 44.404, 66.144, 89.105};
224
225static const double c16_vertical_angle[16]
226 = {-16, 0, -14, 2, -12, 4, -10, 6, -8, 8, -6, 10, -4, 12, -2, 14};
227
228static const double c8_vertical_angle[8] = {-12, 4, -8, 8, -4, 10, 0, 12};
229static const double c1_vertical_angle[8] = {0, 0, 0, 0, 0, 0, 0, 0};
230
231static const double scan_altitude_original_2[16]
232 = {-0.2617993877991494,
233 0.017453292519943295,
234 -0.22689280275926285,
235 0.05235987755982989,
236 -0.19198621771937624,
237 0.08726646259971647,
238 -0.15707963267948966,
239 0.12217304763960307,
240 -0.12217304763960307,
241 0.15707963267948966,
242 -0.08726646259971647,
243 0.19198621771937624,
244 -0.05235987755982989,
245 0.22689280275926285,
246 -0.017453292519943295,
247 0.2617993877991494};
248static const double scan_altitude_original_1[16]
249 = {-0.17453292519943295,
250 0.01160643952576229,
251 -0.15123277968530863,
252 0.03490658503988659,
253 -0.12793263417118436,
254 0.05811946409141117,
255 -0.10471975511965978,
256 0.08141960960553547,
257 -0.08141960960553547,
258 0.10471975511965978,
259 -0.05811946409141117,
260 0.12793263417118436,
261 -0.03490658503988659,
262 0.15123277968530863,
263 -0.01160643952576229,
264 0.17453292519943295};
265
266static const double scan_laser_altitude[8] = {
267 -0.11641346110802178,
268 -0.09302604913129776,
269 -0.06981317007977318,
270 -0.04642575810304917,
271 -0.023212879051524585,
272 -0.0,
273 0.023212879051524585,
274 0.04642575810304917,
275};
276
277static const double scan_laser_altitude_ch32[8] = {
278 -0.24434609527920614,
279 -0.19198621771937624,
280 -0.13962634015954636,
281 -0.08726646259971647,
282 -0.03490658503988659,
283 -0.0,
284 0.03490658503988659,
285 0.08726646259971647,
286};
287
288static const double big_angle[32] = {
289 -17, -16, -15, -14, -13, -12, -11, -10, -9, -8, -7,
290 -6, -5, -4.125, -4, -3.125, -3, -2.125, -2, -1.125, -1, -0.125,
291 0, 0.875, 1, 1.875, 2, 3, 4, 5, 6, 7};
292
293static const double sin_scan_laser_altitude[8] = {
294 std::sin(scan_laser_altitude[0]),
295 std::sin(scan_laser_altitude[1]),
296 std::sin(scan_laser_altitude[2]),
297 std::sin(scan_laser_altitude[3]),
298 std::sin(scan_laser_altitude[4]),
299 std::sin(scan_laser_altitude[5]),
300 std::sin(scan_laser_altitude[6]),
301 std::sin(scan_laser_altitude[7]),
302};
303
304static const double sin_scan_laser_altitude_ch32[8] = {
305 std::sin(scan_laser_altitude_ch32[0]),
306 std::sin(scan_laser_altitude_ch32[1]),
307 std::sin(scan_laser_altitude_ch32[2]),
308 std::sin(scan_laser_altitude_ch32[3]),
309 std::sin(scan_laser_altitude_ch32[4]),
310 std::sin(scan_laser_altitude_ch32[5]),
311 std::sin(scan_laser_altitude_ch32[6]),
312 std::sin(scan_laser_altitude_ch32[7]),
313};
314
315// Pre-compute the sine and cosine for the altitude angles.
316static const double scan_laser_altitude_ch128[32] = {
317 -0.29670597283903605, -0.2792526803190927, -0.2617993877991494,
318 -0.24434609527920614, -0.22689280275926285, -0.20943951023931956,
319 -0.19198621771937624, -0.17453292519943295, -0.15707963267948966,
320 -0.13962634015954636, -0.12217304763960307, -0.10471975511965978,
321 -0.08726646259971647, -0.06981317007977318, -0.05235987755982989,
322 -0.03490658503988659, -0.017453292519943295, 0.0,
323 0.017453292519943295, 0.03490658503988659, 0.05235987755982989,
324 0.06981317007977318, 0.08726646259971647, 0.10471975511965978,
325 0.12217304763960307, 0.13962634015954636, 0.15707963267948966,
326 0.17453292519943295, 0.19198621771937624, 0.20943951023931956,
327 0.22689280275926285, 0.24434609527920614,
328};
329
330static const double sin_scan_laser_altitude_ch128[32] = {
331 std::sin(scan_laser_altitude_ch128[0]),
332 std::sin(scan_laser_altitude_ch128[1]),
333 std::sin(scan_laser_altitude_ch128[2]),
334 std::sin(scan_laser_altitude_ch128[3]),
335 std::sin(scan_laser_altitude_ch128[4]),
336 std::sin(scan_laser_altitude_ch128[5]),
337 std::sin(scan_laser_altitude_ch128[6]),
338 std::sin(scan_laser_altitude_ch128[7]),
339 std::sin(scan_laser_altitude_ch128[8]),
340 std::sin(scan_laser_altitude_ch128[9]),
341 std::sin(scan_laser_altitude_ch128[10]),
342 std::sin(scan_laser_altitude_ch128[11]),
343 std::sin(scan_laser_altitude_ch128[12]),
344 std::sin(scan_laser_altitude_ch128[13]),
345 std::sin(scan_laser_altitude_ch128[14]),
346 std::sin(scan_laser_altitude_ch128[15]),
347 std::sin(scan_laser_altitude_ch128[16]),
348 std::sin(scan_laser_altitude_ch128[17]),
349 std::sin(scan_laser_altitude_ch128[18]),
350 std::sin(scan_laser_altitude_ch128[19]),
351 std::sin(scan_laser_altitude_ch128[20]),
352 std::sin(scan_laser_altitude_ch128[21]),
353 std::sin(scan_laser_altitude_ch128[22]),
354 std::sin(scan_laser_altitude_ch128[23]),
355 std::sin(scan_laser_altitude_ch128[24]),
356 std::sin(scan_laser_altitude_ch128[25]),
357 std::sin(scan_laser_altitude_ch128[26]),
358 std::sin(scan_laser_altitude_ch128[27]),
359 std::sin(scan_laser_altitude_ch128[28]),
360 std::sin(scan_laser_altitude_ch128[29]),
361 std::sin(scan_laser_altitude_ch128[30]),
362 std::sin(scan_laser_altitude_ch128[31]),
363};
364
365static const double scan_mirror_altitude[4] = {
366 -0.0,
367 0.005759586531581287,
368 0.011693705988362009,
369 0.017453292519943295,
370};
371
372static const double sin_scan_mirror_altitude[4] = {
373 std::sin(scan_mirror_altitude[0]),
374 std::sin(scan_mirror_altitude[1]),
375 std::sin(scan_mirror_altitude[2]),
376 std::sin(scan_mirror_altitude[3]),
377};
378
379static const double scan_mirror_altitude_ch128[4] = {
380 0.0,
381 0.004363323129985824,
382 0.008726646259971648,
383 0.013089969389957472,
384};
385
386static const double sin_scan_mirror_altitude_ch128[4] = {
387 std::sin(scan_mirror_altitude_ch128[0]),
388 std::sin(scan_mirror_altitude_ch128[1]),
389 std::sin(scan_mirror_altitude_ch128[2]),
390 std::sin(scan_mirror_altitude_ch128[3]),
391};
392
393static const int POINTS_PER_PACKET_SINGLE_ECHO = 1192; // modify
394static const int POINTS_PER_PACKET_DOUBLE_ECHO = 1188; // modify
395
405typedef struct FiringC32 {
406 uint16_t firing_azimuth[BLOCKS_PER_PACKET];
407 uint16_t azimuth[SCANS_PER_PACKET];
408 double distance[SCANS_PER_PACKET];
409 double intensity[SCANS_PER_PACKET];
410} FiringC32;
411
412union TwoBytes {
413 uint16_t distance;
414 uint8_t bytes[2];
415};
416
417struct RawBlock {
418 uint16_t header;
419 uint16_t rotation; // 0-35999
420 uint8_t data[BLOCK_DATA_SIZE];
421};
422
424 RawBlock blocks[BLOCKS_PER_PACKET];
425 uint32_t time_stamp;
426 uint8_t factory[2];
427};
428
429typedef struct raw_packet {
430 raw_block_t blocks[BLOCKS_PER_PACKET];
431 uint8_t gps_timestamp[10]; // gps时间 1200-1209
432 uint8_t status_type;
435
437 uint8_t uint[2];
438 uint16_t value;
439};
440
441struct Point {
442 uint8_t vertical_line; // 0-127
443 uint8_t azimuth_1;
444 uint8_t azimuth_2;
445 uint8_t distance_1;
446 uint8_t distance_2;
447 uint8_t distance_3;
448 uint8_t intensity;
449};
450
451struct RawPacket {
452 Point points[POINTS_PER_PACKET];
453 uint8_t nodata[3];
454 uint32_t time_stamp;
455 uint8_t factory[2];
456};
457
458struct Firing {
459 // double vertical_angle;
461 double azimuth;
462 double distance;
464};
465
468 // int vertical_line;
469 double azimuth;
470 double distance;
472 double time;
474};
475
477 uint8_t vertical_line; // 0-127
478 uint8_t azimuth_1;
479 uint8_t azimuth_2;
480 uint8_t distance_1;
481 uint8_t distance_2;
482 uint8_t distance_3;
483 uint8_t intensity;
484};
485
487 Point points[POINTS_PER_PACKET];
488 uint8_t nodata[3];
489 uint32_t time_stamp;
490 uint8_t factory[2];
491};
492
493static const float nan = std::numeric_limits<float>::signaling_NaN();
494
497 public:
499 explicit LslidarParser(const Config& config);
500 virtual ~LslidarParser() {}
501
511 virtual void GeneratePointcloud(
512 const std::shared_ptr<LslidarScan>& scan_msg,
513 const std::shared_ptr<apollo::drivers::PointCloud>& out_msg)
514 = 0;
515 virtual void setup();
516
517 // Order point cloud fod IDL by lslidar model
518 virtual void Order(std::shared_ptr<apollo::drivers::PointCloud> cloud) = 0;
519
520 const double get_last_timestamp() {
521 return last_time_stamp_;
522 }
523
524 protected:
525 double scan_altitude[16];
526 float sin_azimuth_table[ROTATION_MAX_UNITS];
527 float cos_azimuth_table[ROTATION_MAX_UNITS];
528 double cos_scan_altitude_caliration[LSC32_SCANS_PER_FIRING];
529 double sin_scan_altitude_caliration[LSC32_SCANS_PER_FIRING];
531 double theat1_s[128];
532 double theat2_s[128];
533 double theat1_c[128];
534 double theat2_c[128];
535 double prism_angle[4];
536 double prism_angle64[4];
543
544 bool is_scan_valid(int rotation, float distance);
545
546 void ComputeCoords(
547 const float& raw_distance,
548 LaserCorrection* corrections,
549 const uint16_t rotation,
550 PointXYZIT* point);
551
552 void ComputeCoords2(
553 int Laser_ring,
554 int Type,
555 const float& raw_distance,
556 LaserCorrection* corrections,
557 const double rotation,
558 PointXYZIT* point);
559}; // class LslidarParser
560
562 public:
563 explicit Lslidar16Parser(const Config& config);
565
567 const std::shared_ptr<apollo::drivers::lslidar::LslidarScan>&
568 scan_msg,
569 const std::shared_ptr<apollo::drivers::PointCloud>& out_msg);
570 void Order(std::shared_ptr<apollo::drivers::PointCloud> cloud);
571
572 private:
573 void Unpack(
574 const LslidarPacket& pkt,
575 std::shared_ptr<apollo::drivers::PointCloud> pc,
576 int packet_number);
577 // Previous Lslidar packet time stamp. (offset to the top hour)
578 double previous_packet_stamp_;
579 uint64_t gps_base_usec_; // full time
580 uint64_t last_packet_time;
581 uint8_t difop_data[PACKET_SIZE];
582 int block_num;
583 int packet_number_;
584 bool read_difop_;
585}; // class Lslidar16Parser
586
588 public:
589 explicit Lslidar32Parser(const Config& config);
591
593 const std::shared_ptr<apollo::drivers::lslidar::LslidarScan>&
594 scan_msg,
595 const std::shared_ptr<apollo::drivers::PointCloud>& out_msg);
596 void Order(std::shared_ptr<apollo::drivers::PointCloud> cloud);
597
598 private:
599 void Unpack(
600 const LslidarPacket& pkt,
601 std::shared_ptr<apollo::drivers::PointCloud> pc,
602 int packet_number);
603 double adjust_angle;
604 double adjust_angle_two;
605 double adjust_angle_three;
606 double adjust_angle_four;
607
608 // Previous Lslidar packet time stamp. (offset to the top hour)
609 double previous_packet_stamp_;
610 uint64_t gps_base_usec_; // full time
611 uint64_t last_packet_time;
612 uint64_t last_point_time;
613 uint8_t difop_data[PACKET_SIZE];
614 int block_num;
615 int lslidar_type;
616 int packet_number_;
617 bool read_difop_;
618
619 double scan_altitude_A[32] = {0};
620 double scan_altitude_C[32] = {0};
621}; // class Lslidar32Parser
622
624 public:
625 explicit LslidarCXV4Parser(const Config& config);
627
628 void decodePacket(const RawPacket_C32* packet);
629
631 const std::shared_ptr<apollo::drivers::lslidar::LslidarScan>&
632 scan_msg,
633 const std::shared_ptr<apollo::drivers::PointCloud>& out_msg);
634 void Order(std::shared_ptr<apollo::drivers::PointCloud> cloud);
635
636 bool checkPacketValidity(const RawPacket_C32* packet);
637
638 private:
639 void Unpack(
640 const LslidarPacket& pkt,
641 std::shared_ptr<apollo::drivers::PointCloud> pc,
642 int packet_number);
643
644 // Previous Lslidar packet time stamp. (offset to the top hour)
645 double previous_packet_stamp_;
646 uint64_t gps_base_usec_; // full time
647 uint64_t last_packet_time;
648 uint64_t last_point_time;
649 uint8_t difop_data[PACKET_SIZE];
650 int block_num;
651 int lslidar_type;
652 int packet_number_;
653 bool read_difop_;
654
655 // double c32_vertical_angle[32] = {0};
656 double cos_scan_altitude[32] = {0};
657 double sin_scan_altitude[32] = {0};
658 double cos_azimuth_table[36000];
659 double sin_azimuth_table[36000];
660 int vertical_angle_; // 雷达垂直角度: 32度,70度,90度
661 FiringC32 firings;
662 double distance_unit_;
663 int lidar_number_;
664 bool is_new_c32w_;
665 bool is_get_scan_altitude_;
666}; // class LslidarCXV4Parser
667
669 public:
670 explicit Lslidar401Parser(const Config& config);
672
674 const std::shared_ptr<LslidarScan>& scan_msg,
675 const std::shared_ptr<apollo::drivers::PointCloud>& out_msg);
676 void Order(std::shared_ptr<apollo::drivers::PointCloud> cloud);
677
678 private:
679 void decodePacket(const raw_packet_t* packet);
680 void Unpack(
681 const LslidarPacket& pkt,
682 std::shared_ptr<apollo::drivers::PointCloud> pc);
683
684 // Previous Lslidar packet time stamp. (offset to the top hour)
685 double previous_packet_stamp_;
686 uint64_t gps_base_usec_; // full time
687 uint64_t last_packet_time;
688 uint8_t difop_data[PACKET_SIZE];
689 int block_num;
690}; // class Lslidar401Parser
691
693 public:
694 explicit LslidarCH16Parser(const Config& config);
696
698 const std::shared_ptr<LslidarScan>& scan_msg,
699 const std::shared_ptr<apollo::drivers::PointCloud>& out_msg);
700 void Order(std::shared_ptr<apollo::drivers::PointCloud> cloud);
701
702 private:
703 void Unpack(
704 int num,
705 const LslidarPacket& pkt,
706 std::shared_ptr<apollo::drivers::PointCloud> pc);
707
708 // Previous Lslidar packet time stamp. (offset to the top hour)
709 double previous_packet_stamp_;
710 uint64_t gps_base_usec_; // full time
711 uint64_t last_packet_time;
712 uint64_t last_point_time;
713 size_t packets_size;
714 uint64_t time_last;
715 uint8_t difop_data[PACKET_SIZE];
716 Firing firings[POINTS_PER_PACKET];
717}; // class LslidarCH16Parser
718
720 public:
721 explicit LslidarCH32Parser(const Config& config);
723
725 const std::shared_ptr<LslidarScan>& scan_msg,
726 const std::shared_ptr<apollo::drivers::PointCloud>& out_msg);
727 void Order(std::shared_ptr<apollo::drivers::PointCloud> cloud);
728
729 private:
730 void Unpack(
731 int num,
732 const LslidarPacket& pkt,
733 std::shared_ptr<apollo::drivers::PointCloud> pc);
734
735 // Previous Lslidar packet time stamp. (offset to the top hour)
736 double previous_packet_stamp_;
737 uint64_t gps_base_usec_; // full time
738 uint64_t last_packet_time;
739 uint64_t last_point_time;
740 size_t packets_size;
741 uint64_t time_last;
742 uint8_t difop_data[PACKET_SIZE];
743 Firing firings[POINTS_PER_PACKET];
744}; // class LslidarCH32Parser
745
747 public:
748 explicit LslidarCH64Parser(const Config& config);
750
752 const std::shared_ptr<LslidarScan>& scan_msg,
753 const std::shared_ptr<apollo::drivers::PointCloud>& out_msg);
754 void Order(std::shared_ptr<apollo::drivers::PointCloud> cloud);
755
756 private:
757 void Unpack(
758 int num,
759 const LslidarPacket& pkt,
760 std::shared_ptr<apollo::drivers::PointCloud> pc);
761
762 // Previous Lslidar packet time stamp. (offset to the top hour)
763 double previous_packet_stamp_;
764 uint64_t gps_base_usec_; // full time
765 uint64_t last_packet_time;
766 uint64_t last_point_time;
767 size_t packets_size;
768 uint64_t time_last;
769 uint8_t difop_data[PACKET_SIZE];
770 Firing firings[POINTS_PER_PACKET];
771}; // class LslidarCH64Parser
772
774 public:
775 explicit LslidarCH64wParser(const Config& config);
777
779 const std::shared_ptr<LslidarScan>& scan_msg,
780 const std::shared_ptr<apollo::drivers::PointCloud>& out_msg);
781 void Order(std::shared_ptr<apollo::drivers::PointCloud> cloud);
782
783 private:
784 void Unpack(
785 int num,
786 const LslidarPacket& pkt,
787 std::shared_ptr<apollo::drivers::PointCloud> pc);
788
789 // Previous Lslidar packet time stamp. (offset to the top hour)
790 double previous_packet_stamp_;
791 uint64_t gps_base_usec_; // full time
792 uint64_t last_packet_time;
793 uint64_t last_point_time;
794 size_t packets_size;
795 uint64_t time_last;
796 double theat1_s[128];
797 double theat2_s[128];
798 double theat1_c[128];
799 double theat2_c[128];
800 double point_time_diff;
801 uint8_t difop_data[PACKET_SIZE];
802 Firing firings[POINTS_PER_PACKET];
803}; // class LslidarCH64Parser
804
806 public:
807 explicit LslidarCH120Parser(const Config& config);
809
811 const std::shared_ptr<LslidarScan>& scan_msg,
812 const std::shared_ptr<PointCloud>& out_msg);
813 void Order(std::shared_ptr<PointCloud> cloud);
814
815 private:
816 void Unpack(
817 int num,
818 const LslidarPacket& pkt,
819 std::shared_ptr<PointCloud> pc);
820
821 // Previous Lslidar packet time stamp. (offset to the top hour)
822 double previous_packet_stamp_;
823 uint64_t gps_base_usec_; // full time
824 uint64_t last_packet_time;
825 uint64_t last_point_time;
826 size_t packets_size;
827 uint64_t time_last;
828 uint8_t difop_data[PACKET_SIZE];
829 Firing firings[POINTS_PER_PACKET];
830}; // class LslidarCH120Parser
831
833 public:
834 explicit LslidarCH128Parser(const Config& config);
836
838 const std::shared_ptr<LslidarScan>& scan_msg,
839 const std::shared_ptr<PointCloud>& out_msg);
840 void Order(std::shared_ptr<PointCloud> cloud);
841
842 private:
843 void Unpack(
844 int num,
845 const LslidarPacket& pkt,
846 std::shared_ptr<PointCloud> pc);
847
848 // Previous Lslidar packet time stamp. (offset to the top hour)
849 double previous_packet_stamp_;
850 uint64_t gps_base_usec_; // full time
851 uint64_t last_packet_time;
852 uint64_t last_point_time;
853 size_t packets_size;
854 double sinTheta_2[128];
855 double prism_angle128[4];
856 uint64_t time_last;
857 uint8_t difop_data[PACKET_SIZE];
858 Firing firings[POINTS_PER_PACKET];
859}; // class LslidarCH128Parser
860
862 public:
863 explicit LslidarCH128X1Parser(const Config& config);
865
867 const std::shared_ptr<LslidarScan>& scan_msg,
868 const std::shared_ptr<PointCloud>& out_msg);
869 void Order(std::shared_ptr<PointCloud> cloud);
870
871 private:
872 void Unpack(
873 int num,
874 const LslidarPacket& pkt,
875 std::shared_ptr<PointCloud> pc);
876
877 // Previous Lslidar packet time stamp. (offset to the top hour)
878 double previous_packet_stamp_;
879 uint64_t gps_base_usec_; // full time
880 uint64_t last_packet_time;
881 uint64_t last_point_time;
882 size_t packets_size;
883 uint8_t difop_data[PACKET_SIZE];
884 Firing firings[POINTS_PER_PACKET];
885}; // class LslidarCH128X1Parser
886
888 public:
889 explicit LslidarLS128S2Parser(const Config& config);
891
893 const std::shared_ptr<LslidarScan>& scan_msg,
894 const std::shared_ptr<PointCloud>& out_msg);
895 void Order(std::shared_ptr<PointCloud> cloud);
896
897 int convertCoordinate(const struct Firing_LS128S2& lidardata);
898
900 const struct Firing_LS128S2& lidardata,
901 std::shared_ptr<PointCloud> out_cloud);
902
903 private:
904 void Unpack(
905 int num,
906 const LslidarPacket& pkt,
907 std::shared_ptr<PointCloud> pc);
908
909 // Previous Lslidar packet time stamp. (offset to the top hour)
910 double previous_packet_stamp_;
911 uint64_t gps_base_usec_; // full time
912 uint64_t last_packet_time;
913 uint64_t last_point_time;
914 size_t packets_size;
915 uint8_t difop_data[PACKET_SIZE];
916 Firing_LS128S2 firings[LS_POINTS_PER_PACKET];
917 int frame_count = 0; // LS系列,叠帧模式会用到
918
919 double cos_table[36000]{};
920 double sin_table[36000]{};
921 double cos_mirror_angle[4]{};
922 double sin_mirror_angle[4]{};
923 double g_fAngleAcc_V = 0.01;
924 bool is_add_frame_ = false;
925 int return_mode = 1;
926 std::shared_ptr<PointCloud> cur_pc;
927 std::shared_ptr<PointCloud> pre_pc;
928 int packet_number_ = 0;
929}; // class LslidarLS128S2Parser
930
932 public:
933 static LslidarParser* CreateParser(Config config);
934};
935
936} // namespace parser
937} // namespace lslidar
938} // namespace drivers
939} // namespace apollo
Calibration class storing entire configuration for the Lslidar
Definition calibration.h:70
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)
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)
void Order(std::shared_ptr< apollo::drivers::PointCloud > cloud)
void GeneratePointcloud(const std::shared_ptr< LslidarScan > &scan_msg, const std::shared_ptr< apollo::drivers::PointCloud > &out_msg)
Set up for data processing.
void Order(std::shared_ptr< PointCloud > cloud)
void GeneratePointcloud(const std::shared_ptr< LslidarScan > &scan_msg, const std::shared_ptr< PointCloud > &out_msg)
void Order(std::shared_ptr< PointCloud > cloud)
void GeneratePointcloud(const std::shared_ptr< LslidarScan > &scan_msg, const std::shared_ptr< PointCloud > &out_msg)
void GeneratePointcloud(const std::shared_ptr< LslidarScan > &scan_msg, const std::shared_ptr< PointCloud > &out_msg)
void Order(std::shared_ptr< apollo::drivers::PointCloud > cloud)
void GeneratePointcloud(const std::shared_ptr< LslidarScan > &scan_msg, const std::shared_ptr< apollo::drivers::PointCloud > &out_msg)
Set up for data processing.
void GeneratePointcloud(const std::shared_ptr< LslidarScan > &scan_msg, const std::shared_ptr< apollo::drivers::PointCloud > &out_msg)
Set up for data processing.
void Order(std::shared_ptr< apollo::drivers::PointCloud > cloud)
void GeneratePointcloud(const std::shared_ptr< LslidarScan > &scan_msg, const std::shared_ptr< apollo::drivers::PointCloud > &out_msg)
Set up for data processing.
void Order(std::shared_ptr< apollo::drivers::PointCloud > cloud)
void GeneratePointcloud(const std::shared_ptr< LslidarScan > &scan_msg, const std::shared_ptr< apollo::drivers::PointCloud > &out_msg)
Set up for data processing.
void Order(std::shared_ptr< apollo::drivers::PointCloud > cloud)
void Order(std::shared_ptr< apollo::drivers::PointCloud > cloud)
void GeneratePointcloud(const std::shared_ptr< apollo::drivers::lslidar::LslidarScan > &scan_msg, const std::shared_ptr< apollo::drivers::PointCloud > &out_msg)
void GeneratePointcloud(const std::shared_ptr< LslidarScan > &scan_msg, const std::shared_ptr< PointCloud > &out_msg)
int convertCoordinate(const struct Firing_LS128S2 &lidardata)
static LslidarParser * CreateParser(Config config)
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.
double cos_scan_altitude_caliration[LSC32_SCANS_PER_FIRING]
virtual void setup()
Set up for on-line operation.
virtual void Order(std::shared_ptr< apollo::drivers::PointCloud > cloud)=0
void ComputeCoords2(int Laser_ring, int Type, const float &raw_distance, LaserCorrection *corrections, const double rotation, PointXYZIT *point)
bool is_scan_valid(int rotation, float distance)
void ComputeCoords(const float &raw_distance, LaserCorrection *corrections, const uint16_t rotation, PointXYZIT *point)
double sin_scan_altitude_caliration[LSC32_SCANS_PER_FIRING]
struct apollo::drivers::lslidar::parser::raw_block raw_block_t
Raw Lslidar data block.
struct apollo::drivers::lslidar::parser::raw_packet raw_packet_t
class register implement
Definition arena_queue.h:37
uint16_t firing_azimuth[BLOCKS_PER_PACKET]
correction values for a single laser
Definition calibration.h:45
uint8_t data[BLOCK_DATA_SIZE]
combine rotation1 and rotation2 together to get 0-35999, divide by 100 to get degrees
uint16_t header
UPPER_BANK or LOWER_BANK
raw_block_t blocks[BLOCKS_PER_PACKET]
used for unpacking the first two data bytes in a block