diff --git a/modules/drivers/lidar/BUILD b/modules/drivers/lidar/BUILD index 1f289ba7b35..81d04bf37bc 100644 --- a/modules/drivers/lidar/BUILD +++ b/modules/drivers/lidar/BUILD @@ -18,6 +18,7 @@ install( "//modules/drivers/lidar/hesai:install", "//modules/drivers/lidar/robosense:install", "//modules/drivers/lidar/velodyne:install", + "//modules/drivers/lidar/lslidar:install", ] ) diff --git a/modules/drivers/lidar/lslidar/BUILD b/modules/drivers/lidar/lslidar/BUILD new file mode 100644 index 00000000000..a1d723e72f6 --- /dev/null +++ b/modules/drivers/lidar/lslidar/BUILD @@ -0,0 +1,27 @@ +load("//tools/install:install.bzl", "install") + +package(default_visibility = ["//visibility:public"]) + +install( + name = "install", + data_dest = "drivers/addition_data/lidar/lslidar", + library_dest = "drivers/lib/lidar/lslidar", + data = [ + ":runtime_data", + ], + targets = [ + "//modules/drivers/lidar/lslidar/driver:liblslidar_driver_component.so", + "//modules/drivers/lidar/lslidar/parser:liblslidar_convert_component.so", + ], +) + +filegroup( + name = "runtime_data", + srcs = glob([ + "conf/*.txt", + "conf/*.conf", + "dag/*.dag", + "launch/*.launch", + "params/*.yaml", + ]), +) diff --git a/modules/drivers/lidar/lslidar/README_cn.md b/modules/drivers/lidar/lslidar/README_cn.md new file mode 100644 index 00000000000..5f7fc59ad16 --- /dev/null +++ b/modules/drivers/lidar/lslidar/README_cn.md @@ -0,0 +1,140 @@ +# **Lslidar LiDAR Driver** + + + +## 1、 工程简介 + + **lslidar** 为镭神在Apollo 8.0平台的雷达驱动集成包。 目前支持*C16，C32，CH16，CH32，CH64，CH64w，CH120，CH128,CH128X1*等型号的雷达。 + + + +## 2、 驱动运行示例 + +#### 2.1 C16 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidar16.launch +或 +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidar16.dag + +默认话题名： + +- 原始点云 -- /apollo/sensor/lslidar16/PointCloud2 +- Scan--/apollo/sensor/lslidar16/Scan +- 运动补偿后点云 -- /apollo/sensor/lslidar16/compensator/PointCloud2 + + +#### 2.2 C32 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidar32.launch +或 +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidar32.dag + +默认话题名： + +- 原始点云 -- /apollo/sensor/lslidar32/PointCloud2 +- Scan--/apollo/sensor/lslidar32/Scan +- 运动补偿后点云 -- /apollo/sensor/lslidar32/compensator/PointCloud2 + + +#### 2.3 N401(删除) + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidar401.launch +或 +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidar401.dag + +默认话题名： + +- 原始点云 -- /apollo/sensor/lslidar401/PointCloud2 +- Scan--/apollo/sensor/lslidar401/Scan + +#### 2.4 CH16 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidarCH16.launch +或 +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH16.dag + +默认话题名： + +- 原始点云 -- /apollo/sensor/lslidarCH16/PointCloud2 +- Scan--/apollo/sensor/lslidarCH16/Scan +- 运动补偿后点云 -- /apollo/sensor/lslidarCH16/compensator/PointCloud2 + +#### 2.5 CH32 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidarCH32.launch +或 +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH32.dag + +默认话题名： + +- 原始点云 -- /apollo/sensor/lslidarCH32/PointCloud2 +- Scan--/apollo/sensor/lslidarCH32/Scan +- 运动补偿后点云 -- /apollo/sensor/lslidarCH32/compensator/PointCloud2 + +#### 2.6 CH64 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidarCH64.launch +或 +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH64.dag + +默认话题名： + +- 原始点云 -- /apollo/sensor/lslidarCH64/PointCloud2 +- Scan--/apollo/sensor/lslidarCH64/Scan +- 运动补偿后点云 -- /apollo/sensor/lslidarCH64/compensator/PointCloud2 + +#### 2.7 CH64w + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidarCH64w.launch +或 +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH64w.dag + +默认话题名： + +- 原始点云 -- /apollo/sensor/lslidarCH64w/PointCloud2 +- Scan--/apollo/sensor/lslidarCH64w/Scan +- 运动补偿后点云 -- /apollo/sensor/lslidarCH64w/compensator/PointCloud2 + +#### 2.8 CH120 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidarCH120.launch +或 +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH120.dag + +默认话题名： + +- 原始点云 -- /apollo/sensor/lslidarCH120/PointCloud2 +- Scan--/apollo/sensor/lslidarCH120/Scan +- 运动补偿后点云 -- /apollo/sensor/lslidarCH120/compensator/PointCloud2 + +#### 2.9 CH128 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidarCH128.launch +或 +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH128.dag + +默认话题名： + +- 原始点云 -- /apollo/sensor/lslidarCH128/PointCloud2 +- Scan--/apollo/sensor/lslidarCH128/Scan +- 运动补偿后点云 -- /apollo/sensor/lslidarCH128/compensator/PointCloud2 + +#### 2.10 CH128X1 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidarCH128X1.launch +或 +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH128X1.dag + +默认话题名： + +- 原始点云 -- /apollo/sensor/lslidarCH128X1/PointCloud2 +- Scan--/apollo/sensor/lslidarCH128X1/Scan +- 运动补偿后点云 -- /apollo/sensor/lslidarCH128X1/compensator/PointCloud2 + + + +### 常见问题 + +- 打印“[lslidar]lslidar poll() timeout, port: 2368‘’ 表示计算平台与雷达网络不通。 + + 可用wireshark查看是否有雷达数据；以及雷达ip和端口号参数设置是否正确。 diff --git a/modules/drivers/lidar/lslidar/README_en.md b/modules/drivers/lidar/lslidar/README_en.md new file mode 100644 index 00000000000..db01baa5c5f --- /dev/null +++ b/modules/drivers/lidar/lslidar/README_en.md @@ -0,0 +1,142 @@ +# **Lslidar LiDAR Driver** + + + +## 1. Overview + +**lslidar** is a lidar driver integration package from LenShen Intelligent System Co. Ltd. for the Apollo 8.0 platform. Currently the *C16, C32, CH16, CH32, CH64, CH64w, CH120, CH128,CH128X1* lidar models are supported. + + + +## 2. Example of driver running + +#### 2.1 C16 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidar16.launch +or +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidar16.dag + +Default topic name: + +- Raw point cloud -- /apollo/sensor/lslidar16/PointCloud2 +- Scan--/apollo/sensor/lslidar16/Scan +- Point cloud after motion compensation -- /apollo/sensor/lslidar16/compensator/PointCloud2 + + +#### 2.2 C32 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidar32.launch +or +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidar32.dag + +Default topic name: + +- Raw point cloud -- /apollo/sensor/lslidar32/PointCloud2 +- Scan--/apollo/sensor/lslidar32/Scan +- Point cloud after motion compensation -- /apollo/sensor/lslidar32/compensator/PointCloud2 + + +#### 2.3 N401(delete) + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidar401.launch +or +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidar401.dag + +Default topic name: + +- Raw point cloud -- /apollo/sensor/lslidar401/PointCloud2 +- Scan--/apollo/sensor/lslidar401/Scan + +#### 2.4 CH16 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidarCH16.launch +or +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH16.dag + +Default topic name: + +- Raw point cloud -- /apollo/sensor/lslidarCH16/PointCloud2 +- Scan--/apollo/sensor/lslidarCH16/Scan +- Point cloud after motion compensation -- /apollo/sensor/lslidarCH16/compensator/PointCloud2 + +#### 2.5 CH32 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidarCH32.launch +or +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH32.dag + +Default topic name: + +- Raw point cloud -- /apollo/sensor/lslidarCH32/PointCloud2 +- Scan--/apollo/sensor/lslidarCH32/Scan +- Point cloud after motion compensation -- /apollo/sensor/lslidarCH32/compensator/PointCloud2 + +#### 2.6 CH64 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidarCH64.launch +or +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH64.dag + +Default topic name: + +- Raw point cloud -- /apollo/sensor/lslidarCH64/PointCloud2 +- Scan--/apollo/sensor/lslidarCH64/Scan +- Point cloud after motion compensation -- /apollo/sensor/lslidarCH64/compensator/PointCloud2 + +#### 2.7 CH64w + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidarCH64w.launch +or +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH64w.dag + +Default topic name: + +- Raw point cloud -- /apollo/sensor/lslidarCH64w/PointCloud2 +- Scan--/apollo/sensor/lslidarCH64w/Scan +- Point cloud after motion compensation -- /apollo/sensor/lslidarCH64w/compensator/PointCloud2 + +#### 2.8 CH120 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidarCH120.launch +or +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH120.dag + +Default topic name: + +- Raw point cloud -- /apollo/sensor/lslidarCH120/PointCloud2 +- Scan--/apollo/sensor/lslidarCH120/Scan +- Point cloud after motion compensation -- /apollo/sensor/lslidarCH120/compensator/PointCloud2 + +#### 2.9 CH128 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidarCH128.launch +or +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH128.dag + +Default topic name: + +- Raw point cloud -- /apollo/sensor/lslidarCH128/PointCloud2 +- Scan--/apollo/sensor/lslidarCH128/Scan +- Point cloud after motion compensation -- /apollo/sensor/lslidarCH128/compensator/PointCloud2 + +#### 2.10 CH128X1 + +cyber_launch start /apollo/modules/drivers/lidar/lslidar/launch/lslidarCH128X1.launch +or +mainboard -d /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH128X1.dag + +Default topic name: + +- Raw point cloud -- /apollo/sensor/lslidarCH128X1/PointCloud2 +- Scan--/apollo/sensor/lslidarCH128X1/Scan +- Point cloud after motion compensation -- /apollo/sensor/lslidarCH128X1/compensator/PointCloud2 + + + +### Frequently asked questions + +- The printout "[lslidar]lslidar poll() timeout, port: 2368" indicates that the computing platform is not connected to the lidar network. + + You can use Wireshark to see if lidar data is available; and if the lidar IP and port number parameters are set correctly. + + diff --git a/modules/drivers/lidar/lslidar/conf/lslidar16_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar16_compensator.pb.txt new file mode 100644 index 00000000000..d4e6fe57241 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidar16_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidar16/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt new file mode 100644 index 00000000000..a12ca204762 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt @@ -0,0 +1,30 @@ +model: LSLIDAR16P +device_ip: "192.168.1.201" +msop_port: 2370 +difop_port: 2371 +return_mode: 1 +degree_mode: 2 #2: 均匀2度校准两列 1://均匀1.33度校准两列 +distance_unit: 0.25 +packet_size: 1206 +time_synchronization: false +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 #雷达转速 10hz:600rpm, 20hz:1200rpm, 5hz:300rpm +convert_channel_name: "/apollo/sensor/lslidar16/PointCloud2" +frame_id: "lslidar16" +scan_channel: "/apollo/sensor/lslidar16/Scan" +min_range: 0.15 +max_range: 150.0 +config_vert: true +scan_start_angle: 0.0 +scan_end_angle: 36000.0 +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 + +calibration: false +calibration_file: "/apollo/modules/drivers/lidar/lslidar/params/LS16_calibration.yaml" +pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 + diff --git a/modules/drivers/lidar/lslidar/conf/lslidar16v4_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar16v4_compensator.pb.txt new file mode 100644 index 00000000000..c39c7999594 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidar16v4_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidar16v4/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidar16v4_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar16v4_conf.pb.txt new file mode 100644 index 00000000000..9d9ba201fef --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidar16v4_conf.pb.txt @@ -0,0 +1,26 @@ +model: LSLIDAR_C16_V4 +device_ip: "192.168.1.200" +msop_port: 2368 +difop_port: 2369 +return_mode: 1 +degree_mode: 2 #2: 均匀1度校准两列 1://均匀0.33度校准两列 +distance_unit: 0.4 +packet_size: 1212 +time_synchronization: false +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 +convert_channel_name: "/apollo/sensor/lslidar16v4/PointCloud2" +frame_id: "lslidar16v4" +scan_channel: "/apollo/sensor/lslidar16v4/Scan" +min_range: 0.15 +max_range: 150.0 +config_vert: true +scan_start_angle: 0.0 +scan_end_angle: 36000.0 +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 +pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 diff --git a/modules/drivers/lidar/lslidar/conf/lslidar1v4_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar1v4_compensator.pb.txt new file mode 100644 index 00000000000..27bea1faf9c --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidar1v4_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidar1v4/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidar1v4_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar1v4_conf.pb.txt new file mode 100644 index 00000000000..7adef29d323 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidar1v4_conf.pb.txt @@ -0,0 +1,26 @@ +model: LSLIDAR_C1_V4 +device_ip: "192.168.1.200" +msop_port: 2368 +difop_port: 2369 +return_mode: 1 +degree_mode: 2 #2: 均匀1度校准两列 1://均匀0.33度校准两列 +distance_unit: 0.4 +packet_size: 1212 +time_synchronization: true +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 +convert_channel_name: "/apollo/sensor/lslidar1v4/PointCloud2" +frame_id: "lslidar1v4" +scan_channel: "/apollo/sensor/lslidar1v4/Scan" +min_range: 0.15 +max_range: 150.0 +config_vert: true +scan_start_angle: 0.0 +scan_end_angle: 36000.0 +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 +pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 diff --git a/modules/drivers/lidar/lslidar/conf/lslidar32_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar32_compensator.pb.txt new file mode 100644 index 00000000000..7dfa5270b15 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidar32_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidar32/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt new file mode 100644 index 00000000000..9b96e476029 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt @@ -0,0 +1,29 @@ +model: LSLIDAR32P +device_ip: "192.168.1.200" +msop_port: 2368 +difop_port: 2369 +return_mode: 1 +degree_mode: 2 #2: 均匀1度校准两列 1://均匀0.33度校准两列 +distance_unit: 0.4 +packet_size: 1206 +time_synchronization: false +add_multicast: false +group_ip: "224.1.1.2" +rpm: 300 #雷达转速 10hz:600rpm, 20hz:1200rpm, 5hz:300rpm +convert_channel_name: "/apollo/sensor/lslidar32/PointCloud2" +frame_id: "lslidar32" +scan_channel: "/apollo/sensor/lslidar32/Scan" +min_range: 0.15 +max_range: 150.0 +config_vert: true +scan_start_angle: 0.0 +scan_end_angle: 36000.0 +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 + +calibration: false +calibration_file: "/apollo/modules/drivers/lidar/lslidar/params/LS32_calibration.yaml" +pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 diff --git a/modules/drivers/lidar/lslidar/conf/lslidar32v4_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar32v4_compensator.pb.txt new file mode 100644 index 00000000000..baa74ccea53 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidar32v4_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidar32v4/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidar32v4_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar32v4_conf.pb.txt new file mode 100644 index 00000000000..29b16fd8bb2 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidar32v4_conf.pb.txt @@ -0,0 +1,27 @@ +model: LSLIDAR_C32_V4 +device_ip: "192.168.1.200" +msop_port: 2368 +difop_port: 2369 +vertical_angle: 32 # 雷达垂直角度： 32度，70度，90度 +return_mode: 1 +degree_mode: 2 #2: 均匀1度校准两列 1://均匀0.33度校准两列 +distance_unit: 0.4 +packet_size: 1212 +time_synchronization: false +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 +convert_channel_name: "/apollo/sensor/lslidar32v4/PointCloud2" +frame_id: "lslidar32v4" +scan_channel: "/apollo/sensor/lslidar32v4/Scan" +min_range: 0.15 +max_range: 150.0 +config_vert: true +scan_start_angle: 0.0 +scan_end_angle: 36000.0 +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 +pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 diff --git a/modules/drivers/lidar/lslidar/conf/lslidar401_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar401_compensator.pb.txt new file mode 100644 index 00000000000..a521ce085b1 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidar401_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidar401/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidar401_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar401_conf.pb.txt new file mode 100644 index 00000000000..24d42231a22 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidar401_conf.pb.txt @@ -0,0 +1,16 @@ +model: LSLIDAR401 +device_ip: "192.168.1.222" +msop_port: 2368 +packet_size: 1206 +time_synchronization: false +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 +convert_channel_name: "/apollo/sensor/lslidar401/PointCloud2" +frame_id: "lslidar401" +scan_channel: "/apollo/sensor/lslidar401/Scan" +min_range: 0.3 +max_range: 100.0 +scan_start_angle: 0.0 +scan_end_angle: 36000.0 + diff --git a/modules/drivers/lidar/lslidar/conf/lslidar8v4_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar8v4_compensator.pb.txt new file mode 100644 index 00000000000..b5ce000184e --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidar8v4_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidar8v4/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidar8v4_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidar8v4_conf.pb.txt new file mode 100644 index 00000000000..9c67b80cef8 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidar8v4_conf.pb.txt @@ -0,0 +1,26 @@ +model: LSLIDAR_C8_V4 +device_ip: "192.168.1.200" +msop_port: 2368 +difop_port: 2369 +return_mode: 1 +degree_mode: 2 #2: 均匀1度校准两列 1://均匀0.33度校准两列 +distance_unit: 0.4 +packet_size: 1212 +time_synchronization: true +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 +convert_channel_name: "/apollo/sensor/lslidar8v4/PointCloud2" +frame_id: "lslidar8v4" +scan_channel: "/apollo/sensor/lslidar8v4/Scan" +min_range: 0.15 +max_range: 150.0 +config_vert: true +scan_start_angle: 0.0 +scan_end_angle: 36000.0 +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 +pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 diff --git a/modules/drivers/lidar/lslidar/conf/lslidarCH120_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarCH120_compensator.pb.txt new file mode 100644 index 00000000000..d4ce6cce742 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarCH120_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidarCH120/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidarCH120_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarCH120_conf.pb.txt new file mode 100644 index 00000000000..540c5ac402c --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarCH120_conf.pb.txt @@ -0,0 +1,23 @@ +model: LSLIDAR_CH120 +device_ip: "192.168.1.210" +msop_port: 2378 +difop_port: 2379 +packet_size: 1206 +time_synchronization: true +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 +convert_channel_name: "/apollo/sensor/lslidarCH120/PointCloud2" +frame_id: "lslidarCH120" +scan_channel: "/apollo/sensor/lslidarCH120/Scan" +min_range: 0.15 +max_range: 150.0 +calibration: false +calibration_file: "/apollo/modules/drivers/lidar/lslidar/params/LSCH120_calibration.yaml" + +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 +pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 diff --git a/modules/drivers/lidar/lslidar/conf/lslidarCH128X1_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarCH128X1_compensator.pb.txt new file mode 100644 index 00000000000..8e4620ea7af --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarCH128X1_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidarCH128X1/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidarCH128X1_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarCH128X1_conf.pb.txt new file mode 100644 index 00000000000..111ff7cd154 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarCH128X1_conf.pb.txt @@ -0,0 +1,23 @@ +model: LSLIDAR_CH128X1 +device_ip: "192.168.1.200" +msop_port: 2368 +difop_port: 2369 +packet_size: 1206 +time_synchronization: false +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 +convert_channel_name: "/apollo/sensor/lslidarCH128X1/PointCloud2" +frame_id: "lslidarCH128X1" +scan_channel: "/apollo/sensor/lslidarCH128X1/Scan" +min_range: 0.15 +max_range: 150.0 +calibration: false +calibration_file: "/apollo/modules/drivers/lidar/lslidar/params/LSCH128X1_calibration.yaml" + +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 +pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 diff --git a/modules/drivers/lidar/lslidar/conf/lslidarCH128_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarCH128_compensator.pb.txt new file mode 100644 index 00000000000..97d429d96bf --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarCH128_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidarCH128/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidarCH128_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarCH128_conf.pb.txt new file mode 100644 index 00000000000..23463b1866b --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarCH128_conf.pb.txt @@ -0,0 +1,23 @@ +model: LSLIDAR_CH128 +device_ip: "192.168.1.102" +msop_port: 2368 +difop_port: 2369 +packet_size: 1206 +time_synchronization: false +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 +convert_channel_name: "/apollo/sensor/lslidarCH128/PointCloud2" +frame_id: "lslidarCH128" +scan_channel: "/apollo/sensor/lslidarCH128/Scan" +min_range: 0.15 +max_range: 150.0 +calibration: false +calibration_file: "/apollo/modules/drivers/lidar/lslidar/params/LSCH128_calibration.yaml" + +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 +pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 diff --git a/modules/drivers/lidar/lslidar/conf/lslidarCH16_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarCH16_compensator.pb.txt new file mode 100644 index 00000000000..0388fc68d79 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarCH16_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidarCH16/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidarCH16_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarCH16_conf.pb.txt new file mode 100644 index 00000000000..0c28b79be6a --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarCH16_conf.pb.txt @@ -0,0 +1,23 @@ +model: LSLIDAR_CH16 +device_ip: "192.168.1.200" +msop_port: 2368 +difop_port: 2369 +packet_size: 1206 +time_synchronization: true +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 +convert_channel_name: "/apollo/sensor/lslidarCH16/PointCloud2" +frame_id: "lslidarCH16" +scan_channel: "/apollo/sensor/lslidarCH16/Scan" +min_range: 0.15 +max_range: 150.0 +calibration: false +calibration_file: "/apollo/modules/drivers/lidar/lslidar/params/LSCH16_calibration.yaml" + +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 +pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 diff --git a/modules/drivers/lidar/lslidar/conf/lslidarCH32_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarCH32_compensator.pb.txt new file mode 100644 index 00000000000..8e014527658 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarCH32_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidarCH32/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidarCH32_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarCH32_conf.pb.txt new file mode 100644 index 00000000000..4b2e024c01b --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarCH32_conf.pb.txt @@ -0,0 +1,24 @@ +model: LSLIDAR_CH32 +device_ip: "192.168.1.200" +msop_port: 2368 +difop_port: 2369 +degree_mode: 1 +packet_size: 1206 +time_synchronization: true +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 +convert_channel_name: "/apollo/sensor/lslidarCH32/PointCloud2" +frame_id: "lslidarCH32" +scan_channel: "/apollo/sensor/lslidarCH32/Scan" +min_range: 0.15 +max_range: 150.0 +calibration: false +calibration_file: "/apollo/modules/drivers/lidar/lslidar/params/LSCH32_calibration.yaml" + +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 +pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 diff --git a/modules/drivers/lidar/lslidar/conf/lslidarCH64_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarCH64_compensator.pb.txt new file mode 100644 index 00000000000..f046c14ac9a --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarCH64_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidarCH64/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidarCH64_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarCH64_conf.pb.txt new file mode 100644 index 00000000000..f7fddab63d1 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarCH64_conf.pb.txt @@ -0,0 +1,23 @@ +model: LSLIDAR_CH64 +device_ip: "192.168.1.200" +msop_port: 2368 +difop_port: 2369 +packet_size: 1206 +time_synchronization: true +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 +convert_channel_name: "/apollo/sensor/lslidarCH64/PointCloud2" +frame_id: "lslidarCH64" +scan_channel: "/apollo/sensor/lslidarCH64/Scan" +min_range: 0.15 +max_range: 150.0 +calibration: true +calibration_file: "/apollo/modules/drivers/lidar/lslidar/params/LSCH64_calibration.yaml" + +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 +pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 diff --git a/modules/drivers/lidar/lslidar/conf/lslidarCH64w_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarCH64w_compensator.pb.txt new file mode 100644 index 00000000000..b25c8a2a5ac --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarCH64w_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidarCH64w/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidarCH64w_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarCH64w_conf.pb.txt new file mode 100644 index 00000000000..6c3f9268887 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarCH64w_conf.pb.txt @@ -0,0 +1,23 @@ +model: LSLIDAR_CH64w +device_ip: "192.168.1.200" +msop_port: 2370 +difop_port: 2371 +packet_size: 1206 +time_synchronization: false +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 +convert_channel_name: "/apollo/sensor/lslidarCH64w/PointCloud2" +frame_id: "lslidarCH64w" +scan_channel: "/apollo/sensor/lslidarCH64w/Scan" +min_range: 0.15 +max_range: 150.0 +calibration: false +calibration_file: "/apollo/modules/drivers/lidar/lslidar/params/LSCH64w_calibration.yaml" + +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 +pcap_path: "/ch64w_201.pcap" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 diff --git a/modules/drivers/lidar/lslidar/conf/lslidarLS128S2_compensator.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarLS128S2_compensator.pb.txt new file mode 100644 index 00000000000..7c26fdbd1f6 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarLS128S2_compensator.pb.txt @@ -0,0 +1,3 @@ +world_frame_id: "world" +transform_query_timeout: 0.02 +output_channel: "/apollo/sensor/lslidarLS128S2/compensator/PointCloud2" diff --git a/modules/drivers/lidar/lslidar/conf/lslidarLS128S2_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidarLS128S2_conf.pb.txt new file mode 100644 index 00000000000..bc69023cbab --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidarLS128S2_conf.pb.txt @@ -0,0 +1,25 @@ +model: LSLIDAR_LS128S2 +device_ip: "192.168.1.200" +msop_port: 2368 +difop_port: 2369 +packet_size: 1206 +time_synchronization: false +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 +convert_channel_name: "/apollo/sensor/lslidarLS128S2/PointCloud2" +frame_id: "lslidarLS128S2" +scan_channel: "/apollo/sensor/lslidarLS128S2/Scan" +min_range: 0.15 +max_range: 500.0 +scan_start_angle: -60.0 +scan_end_angle: 60.0 +calibration: false +calibration_file: "/apollo/modules/drivers/lidar/lslidar/params/LS128S2_calibration.yaml" + +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 +pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 diff --git a/modules/drivers/lidar/lslidar/conf/lslidartest_conf.pb.txt b/modules/drivers/lidar/lslidar/conf/lslidartest_conf.pb.txt new file mode 100644 index 00000000000..0bb207bb4f9 --- /dev/null +++ b/modules/drivers/lidar/lslidar/conf/lslidartest_conf.pb.txt @@ -0,0 +1,29 @@ +model: LSLIDAR_C1_V4 +device_ip: "192.168.1.200" +msop_port: 2368 +difop_port: 2369 +return_mode: 1 +degree_mode: 2 #2: 均匀2度校准两列 1://均匀1.33度校准两列 +distance_unit: 0.4 +packet_size: 1212 +time_synchronization: true +add_multicast: false +group_ip: "224.1.1.2" +rpm: 600 #雷达转速 10hz:600rpm, 20hz:1200rpm, 5hz:300rpm +convert_channel_name: "/apollo/sensor/lslidar1v4/PointCloud2" +frame_id: "lslidar16" +scan_channel: "/apollo/sensor/lslidar1v4/Scan" +min_range: 0.15 +max_range: 150.0 +config_vert: true +scan_start_angle: 0.0 +scan_end_angle: 36000.0 +#要屏蔽的矩形区域参数 +bottom_left_x: 0.0 #左下 x值 +bottom_left_y: 0.0 #左下 y值 +top_right_x: 0.0 #右上 x值 +top_right_y: 0.0 #右上 y值 + +#calibration: false +#calibration_file: "/apollo/modules/drivers/lidar/lslidar/params/LS16_calibration.yaml" +pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空 diff --git a/modules/drivers/lidar/lslidar/dag/lslidar16.dag b/modules/drivers/lidar/lslidar/dag/lslidar16.dag new file mode 100644 index 00000000000..9456fb17527 --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidar16.dag @@ -0,0 +1,27 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c16" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c16" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt" + readers { + channel: "/apollo/sensor/lslidar16/Scan" + } + } + } +} + diff --git a/modules/drivers/lidar/lslidar/dag/lslidar16_32_128X1.dag b/modules/drivers/lidar/lslidar/dag/lslidar16_32_128X1.dag new file mode 100644 index 00000000000..83e7b720e38 --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidar16_32_128X1.dag @@ -0,0 +1,123 @@ +#---------------------------------------------------------------------------------------------------- +#c16 +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c16" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c16" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidar16/Scan"} + } + } +} + +#module_config { +# module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/compensator/liblslidar_compensator_component.so" +# components { +# class_name : "CompensatorComponent" +# config { +# name : "lslidar16_compensator_c16" +# config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16_compensator.pb.txt" +# readers {channel: "/apollo/sensor/lslidar16/PointCloud2"} +# } +# } +#} + +#---------------------------------------------------------------------------------------------------- +#c32 +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c32" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c32" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidar32/Scan"} + } + } +} + +#module_config { +# module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/compensator/liblslidar_compensator_component.so" +# components { +# class_name : "CompensatorComponent" +# config { +# name : "lslidar32_compensator_c32" +# config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32_compensator.pb.txt" +# readers {channel: "/apollo/sensor/lslidar32/PointCloud2"} +# } +# } +#} + + +#---------------------------------------------------------------------------------------------------- +#ch128x1 +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_ch128x1" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH128X1_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_ch128x1" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH128X1_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidarCH64w/Scan"} + } + } +} + +#module_config { +# module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/compensator/liblslidar_compensator_component.so" +# components { +# class_name : "CompensatorComponent" +# config { +# name : "lslidarCH64w_compensator_ch128x1" +# config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH128X1_compensator.pb.txt" +# readers {channel: "/apollo/sensor/lslidarCH64w/PointCloud2"} +# } +# } +#} + + + + + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidar16_32_32w.dag b/modules/drivers/lidar/lslidar/dag/lslidar16_32_32w.dag new file mode 100644 index 00000000000..dfa2e2e1275 --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidar16_32_32w.dag @@ -0,0 +1,122 @@ +#---------------------------------------------------------------------------------------------------- +#c16 +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c16" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c16" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidar16/Scan"} + } + } +} + +#module_config { +# module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/compensator/liblslidar_compensator_component.so" +# components { +# class_name : "CompensatorComponent" +# config { +# name : "lslidar16_compensator_c16" +# config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16_compensator.pb.txt" +# readers {channel: "/apollo/sensor/lslidar16/PointCloud2"} +# } +# } +#} + +#---------------------------------------------------------------------------------------------------- +#c32 +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c32" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c32" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidar32/Scan"} + } + } +} + +#module_config { +# module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/compensator/liblslidar_compensator_component.so" +# components { +# class_name : "CompensatorComponent" +# config { +# name : "lslidar32_compensator_c32" +# config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32_compensator.pb.txt" +# readers {channel: "/apollo/sensor/lslidar32/PointCloud2"} +# } +# } +#} + +#---------------------------------------------------------------------------------------------------- +#c32w +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c32v4" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32v4_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c32v4" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32v4_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidar32v4/Scan"} + } + } +} + +#module_config { +# module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/compensator/liblslidar_compensator_component.so" +# components { +# class_name : "CompensatorComponent" +# config { +# name : "lslidarCH64w_compensator_c32v4" +# config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32v4_compensator.pb.txt" +# readers {channel: "/apollo/sensor/lslidar32v4/PointCloud2"} +# } +# } +#} +# + + + + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidar16_32_64w.dag b/modules/drivers/lidar/lslidar/dag/lslidar16_32_64w.dag new file mode 100644 index 00000000000..2d7daa3841b --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidar16_32_64w.dag @@ -0,0 +1,122 @@ +#---------------------------------------------------------------------------------------------------- +#c16 +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c16" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c16" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidar16/Scan"} + } + } +} + +#module_config { +# module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/compensator/liblslidar_compensator_component.so" +# components { +# class_name : "CompensatorComponent" +# config { +# name : "lslidar16_compensator_c16" +# config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16_compensator.pb.txt" +# readers {channel: "/apollo/sensor/lslidar16/PointCloud2"} +# } +# } +#} + +#---------------------------------------------------------------------------------------------------- +#c32 +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c32" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c32" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidar32/Scan"} + } + } +} + +#module_config { +# module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/compensator/liblslidar_compensator_component.so" +# components { +# class_name : "CompensatorComponent" +# config { +# name : "lslidar32_compensator_c32" +# config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32_compensator.pb.txt" +# readers {channel: "/apollo/sensor/lslidar32/PointCloud2"} +# } +# } +#} + +#---------------------------------------------------------------------------------------------------- +#ch64w +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_ch64w" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH64w_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_ch64w" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH64w_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidarCH64w/Scan"} + } + } +} + +#module_config { +# module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/compensator/liblslidar_compensator_component.so" +# components { +# class_name : "CompensatorComponent" +# config { +# name : "lslidarCH64w_compensator_ch64w" +# config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH64w_compensator.pb.txt" +# readers {channel: "/apollo/sensor/lslidarCH64w/PointCloud2"} +# } +# } +#} + + + + + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidar16_32_64w_128X1.dag b/modules/drivers/lidar/lslidar/dag/lslidar16_32_64w_128X1.dag new file mode 100644 index 00000000000..5a187c3890f --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidar16_32_64w_128X1.dag @@ -0,0 +1,161 @@ +#---------------------------------------------------------------------------------------------------- +#c16 +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c16" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c16" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidar16/Scan"} + } + } +} + +#module_config { +# module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/compensator/liblslidar_compensator_component.so" +# components { +# class_name : "CompensatorComponent" +# config { +# name : "lslidar16_compensator_c16" +# config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16_compensator.pb.txt" +# readers {channel: "/apollo/sensor/lslidar16/PointCloud2"} +# } +# } +#} + +#---------------------------------------------------------------------------------------------------- +#c32 +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c32" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c32" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidar32/Scan"} + } + } +} + +#module_config { +# module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/compensator/liblslidar_compensator_component.so" +# components { +# class_name : "CompensatorComponent" +# config { +# name : "lslidar32_compensator_c32" +# config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32_compensator.pb.txt" +# readers {channel: "/apollo/sensor/lslidar32/PointCloud2"} +# } +# } +#} + + +#---------------------------------------------------------------------------------------------------- +#ch64w +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_ch64w" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH64w_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_ch64w" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH64w_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidarCH64w/Scan"} + } + } +} + +#module_config { +# module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/compensator/liblslidar_compensator_component.so" +# components { +# class_name : "CompensatorComponent" +# config { +# name : "lslidarCH64w_compensator_ch64w" +# config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH64w_compensator.pb.txt" +# readers {channel: "/apollo/sensor/lslidarCH64w/PointCloud2"} +# } +# } +#} + +#---------------------------------------------------------------------------------------------------- +#ch128x1 +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_ch128x1" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH128X1_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_ch128x1" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH128X1_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidarCH64w/Scan"} + } + } +} + +#module_config { +# module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/compensator/liblslidar_compensator_component.so" +# components { +# class_name : "CompensatorComponent" +# config { +# name : "lslidarCH64w_compensator_ch128x1" +# config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH128X1_compensator.pb.txt" +# readers {channel: "/apollo/sensor/lslidarCH64w/PointCloud2"} +# } +# } +#} + + + + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidar16v4.dag b/modules/drivers/lidar/lslidar/dag/lslidar16v4.dag new file mode 100644 index 00000000000..777cec747c4 --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidar16v4.dag @@ -0,0 +1,28 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c16_v4" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16v4_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c16_v4" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar16v4_conf.pb.txt" + readers { + channel: "/apollo/sensor/lslidar16v4/Scan" + } + } + } +} + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidar1v4.dag b/modules/drivers/lidar/lslidar/dag/lslidar1v4.dag new file mode 100644 index 00000000000..fc63fc77165 --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidar1v4.dag @@ -0,0 +1,27 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c1_v4" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar1v4_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c1_v4" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar1v4_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidar1v4/Scan"} + } + } +} + + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidar32.dag b/modules/drivers/lidar/lslidar/dag/lslidar32.dag new file mode 100644 index 00000000000..8e1545640a3 --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidar32.dag @@ -0,0 +1,26 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c32" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c32" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidar32/Scan"} + } + } +} + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidar32v4.dag b/modules/drivers/lidar/lslidar/dag/lslidar32v4.dag new file mode 100644 index 00000000000..65e9eae55ab --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidar32v4.dag @@ -0,0 +1,29 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c32_v4" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32v4_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c32_v4" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar32v4_conf.pb.txt" + readers { + channel: "/apollo/sensor/lslidar32v4/Scan" + } + } + } +} + + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidar8v4.dag b/modules/drivers/lidar/lslidar/dag/lslidar8v4.dag new file mode 100644 index 00000000000..0e018c01c0c --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidar8v4.dag @@ -0,0 +1,26 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_c8_v4" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar8v4_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_c8_v4" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidar8v4_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidar8v4/Scan"} + } + } +} + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidarCH128.dag b/modules/drivers/lidar/lslidar/dag/lslidarCH128.dag new file mode 100644 index 00000000000..95d0339fff5 --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidarCH128.dag @@ -0,0 +1,27 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_ch128" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH128_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_ch128" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH128_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidarCH128/Scan"} + } + } +} + + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidarCH128X1.dag b/modules/drivers/lidar/lslidar/dag/lslidarCH128X1.dag new file mode 100644 index 00000000000..0c968ffddfd --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidarCH128X1.dag @@ -0,0 +1,27 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_ch128x1" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH128X1_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_ch128x1" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH128X1_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidarCH128X1/Scan"} + } + } +} + + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidarCH16.dag b/modules/drivers/lidar/lslidar/dag/lslidarCH16.dag new file mode 100644 index 00000000000..db18e4550f1 --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidarCH16.dag @@ -0,0 +1,28 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_ch16" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH16_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_ch16" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH16_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidarCH16/Scan"} + } + } +} + + + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidarCH32.dag b/modules/drivers/lidar/lslidar/dag/lslidarCH32.dag new file mode 100644 index 00000000000..20e18f84bc3 --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidarCH32.dag @@ -0,0 +1,27 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_ch32" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH32_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_ch32" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH32_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidarCH32/Scan"} + } + } +} + + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidarCH64.dag b/modules/drivers/lidar/lslidar/dag/lslidarCH64.dag new file mode 100644 index 00000000000..65f99a39982 --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidarCH64.dag @@ -0,0 +1,27 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_ch64" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH64_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_ch64" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH64_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidarCH64/Scan"} + } + } +} + + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidarCH64w.dag b/modules/drivers/lidar/lslidar/dag/lslidarCH64w.dag new file mode 100644 index 00000000000..9af6b6b859f --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidarCH64w.dag @@ -0,0 +1,27 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_ch64w" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH64w_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_ch64w" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH64w_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidarCH64w/Scan"} + } + } +} + + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidarLS128S2.dag b/modules/drivers/lidar/lslidar/dag/lslidarLS128S2.dag new file mode 100644 index 00000000000..9ba0075a0df --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidarLS128S2.dag @@ -0,0 +1,27 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver_ls128s2" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarLS128S2_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert_ls128s2" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarLS128S2_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidarLS128S2/Scan"} + } + } +} + + + diff --git a/modules/drivers/lidar/lslidar/dag/lslidar_fusion.dag b/modules/drivers/lidar/lslidar/dag/lslidar_fusion.dag new file mode 100644 index 00000000000..c9fb0c5c9e9 --- /dev/null +++ b/modules/drivers/lidar/lslidar/dag/lslidar_fusion.dag @@ -0,0 +1,33 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/driver/liblslidar_driver_component.so" + + components { + class_name : "LslidarDriverComponent" + config { + name : "lslidar_driver" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH64w_conf.pb.txt" + } + } +} + +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/lslidar/parser/liblslidar_convert_component.so" + + components { + class_name : "LslidarConvertComponent" + config { + name : "lslidar_convert" + config_file_path : "/apollo/modules/drivers/lidar/lslidar/conf/lslidarCH64w_conf.pb.txt" + readers {channel: "/apollo/sensor/lslidarCH64w/Scan"} + } + } +} + + + + + + + + + diff --git a/modules/drivers/lidar/lslidar/driver/BUILD b/modules/drivers/lidar/lslidar/driver/BUILD new file mode 100755 index 00000000000..3e1e04f1bfa --- /dev/null +++ b/modules/drivers/lidar/lslidar/driver/BUILD @@ -0,0 +1,59 @@ +load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library") +load("//tools:cpplint.bzl", "cpplint") +load("//tools/install:install.bzl", "install") + + +package(default_visibility = ["//visibility:public"]) + + + +install( + name = "install", + library_dest = "drivers/lib/lidar/lslidar/driver", + targets = [ + ":liblslidar_driver_component.so", + ], +) + +cc_binary( + name = "liblslidar_driver_component.so", + linkshared = True, + linkstatic = True, + deps = [":lslidar_driver_component_lib"], +) + +cc_library( + name = "lslidar_driver_component_lib", + srcs = ["lslidar_driver_component.cc"], + hdrs = ["lslidar_driver_component.h"], + copts = ['-DMODULE_NAME=\\"lslidar\\"'], + deps = [ + "//cyber", + "//modules/common/util:util_tool", + "//modules/drivers/lidar/lslidar/driver", + ], + alwayslink = True, +) + +cc_library( + name = "driver", + srcs = [ + "driver.cc", + "input.cc", + ], + hdrs = [ + "driver.h", + "input.h", + ], + copts = ['-DMODULE_NAME=\\"lslidar\\"'], + linkopts = [ + "-lpcap", + ], + deps = [ + "//cyber", + "//modules/common/util:util_tool", + "//modules/drivers/lidar/lslidar/proto:config_cc_proto", + ], +) + +cpplint() diff --git a/modules/drivers/lidar/lslidar/driver/driver.cc b/modules/drivers/lidar/lslidar/driver/driver.cc new file mode 100755 index 00000000000..fd5825b6cd9 --- /dev/null +++ b/modules/drivers/lidar/lslidar/driver/driver.cc @@ -0,0 +1,512 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/driver/driver.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +LslidarDriver::~LslidarDriver() { + if (difop_thread_.joinable()) { + difop_thread_.join(); + } +} + +void LslidarDriver::Init() { + int packets_rate; + + if (config_.model() == LSLIDAR16P || config_.model() == LSLIDAR_C16_V4) { + packets_rate = 840; + } else if (config_.model() == LSLIDAR_C8_V4) { + packets_rate = 420; + } else if (config_.model() == LSLIDAR_C1_V4) { + packets_rate = 420; + } else if (config_.model() == LSLIDAR_C32_V4) { + packets_rate = 1693; // 64000/384 + } else if (config_.model() == LSLIDAR32P) { + if (1 == config_.degree_mode()) packets_rate = 1693; // 65000/384 + if (2 == config_.degree_mode()) packets_rate = 1700; // 64000/384 + } else if (config_.model() == LSLIDAR_CH16) { + packets_rate = 3571; + } else if (config_.model() == LSLIDAR_CH32) { + packets_rate = 3512; + } else if (config_.model() == LSLIDAR_CH64) { + packets_rate = 3388; + } else if (config_.model() == LSLIDAR_CH64w) { + packets_rate = 11228; + } else if (config_.model() == LSLIDAR_CH128) { + packets_rate = 3571; + } else if (config_.model() == LSLIDAR_CH128X1) { + packets_rate = 6720; + } else if (config_.model() == LSLIDAR_LS128S2) { + packets_rate = 12440; + } else { + packets_rate = 4561; + } + + // default number of packets for each scan is a single revolution + if (config_.model() == LSLIDAR16P || config_.model() == LSLIDAR32P || + config_.model() == LSLIDAR_C32_V4 || config_.model() == LSLIDAR_C16_V4 || + config_.model() == LSLIDAR_C8_V4 || config_.model() == LSLIDAR_C1_V4) + config_.set_npackets(static_cast( + ceil(packets_rate * 60 / config_.rpm()) * config_.return_mode() * 1.1)); + else + config_.set_npackets( + static_cast(ceil(packets_rate * 60 / config_.rpm() * 1.1))); + + AERROR << "config_.pcap_path(): " << config_.pcap_path(); + + if (!config_.pcap_path().empty()) { + if (config_.model() == LSLIDAR_C32_V4 || + config_.model() == LSLIDAR_C16_V4 || config_.model() == LSLIDAR_C8_V4 || + config_.model() == LSLIDAR_C1_V4) { + // open Lslidar input device + + input_.reset(new InputPCAP(config_.msop_port(), config_.device_ip(), + config_.packet_size(), packets_rate, + config_.pcap_path())); // 数据包 + positioning_input_.reset(new InputPCAP(config_.difop_port(), + config_.device_ip(), 1206, 1, + config_.pcap_path())); // 设备包 + } else { + // open Lslidar input device + input_.reset(new InputPCAP(config_.msop_port(), config_.device_ip(), + config_.packet_size(), packets_rate, + config_.pcap_path())); // 数据包 + positioning_input_.reset(new InputPCAP( + config_.difop_port(), config_.device_ip(), config_.packet_size(), 1, + config_.pcap_path())); // 设备包 + } + } else { + if (config_.model() == LSLIDAR_C32_V4 || + config_.model() == LSLIDAR_C16_V4 || config_.model() == LSLIDAR_C8_V4 || + config_.model() == LSLIDAR_C1_V4) { + // open Lslidar input device + input_.reset(new InputSocket(config_.msop_port(), config_.device_ip(), + config_.packet_size())); // 数据包 + positioning_input_.reset(new InputSocket( + config_.difop_port(), config_.device_ip(), 1206)); // 设备包 + } else { + // open Lslidar input device + input_.reset(new InputSocket(config_.msop_port(), config_.device_ip(), + config_.packet_size())); // 数据包 + positioning_input_.reset( + new InputSocket(config_.difop_port(), config_.device_ip(), + config_.packet_size())); // 设备包 + } + } + difop_thread_ = std::thread(&LslidarDriver::difopPoll, this); +} + +/** poll the device + * + * @returns true unless end of file reached + */ +bool LslidarDriver::Poll( + const std::shared_ptr &scan) { + // Allocate a new shared pointer for zero-copy sharing with other nodelets. + int poll_result = PollStandard(scan); + if (poll_result < 0) return false; + + if (scan->firing_pkts().empty()) { + AINFO << "Get an empty scan from port: " << config_.msop_port(); + return false; + } + // publish message using time of last packet read + ADEBUG << "Publishing a full Lslidar scan."; + LslidarPacket *packet = scan->add_difop_pkts(); + std::unique_lock lock(mutex_); + packet->set_data(bytes, FIRING_DATA_PACKET_SIZE); + scan->mutable_header()->set_timestamp_sec(gps_time); + ADEBUG << "**************************************************************GPS " + "time: " + << gps_time; + scan->mutable_header()->set_frame_id(config_.frame_id()); + scan->set_model(config_.model()); + scan->set_basetime(gps_time); + scan->mutable_header()->set_timestamp_sec(gps_time); + return true; +} + +int LslidarDriver::PollStandard( + std::shared_ptr scan) { + // Since the lslidar delivers data at a very high rate, keep reading and + // publishing scans as fast as possible. + + if (config_.model() == LSLIDAR16P || config_.model() == LSLIDAR32P || + config_.model() == LSLIDAR_C32_V4 || config_.model() == LSLIDAR_C16_V4 || + config_.model() == LSLIDAR_C8_V4 || config_.model() == LSLIDAR_C1_V4) { + // Find the 0 degree packet, in order to fix the 0 degree angle position + if (scan_fill) { + LslidarPacket *packet = scan->add_firing_pkts(); + int PKT_DATA_LENGTH = 1212; + void *data_ptr = malloc(PKT_DATA_LENGTH); + memcpy(data_ptr, + reinterpret_cast( + const_cast(scan_start.data().c_str())), + PKT_DATA_LENGTH); + packet->set_data(data_ptr, PKT_DATA_LENGTH); + packet->set_stamp((scan_start.stamp())); + AINFO << "scan->firing_pkts_size(): " << scan->firing_pkts_size(); + } + + scan_fill = false; + int i = 1; + while (scan->firing_pkts_size() < config_.npackets()) { + // config_.npackets() + LslidarPacket *packet = scan->add_firing_pkts(); + while (true) { + // keep reading until full packet received + int rc = input_->GetPacket(packet); + + if (rc == 0) { + if (!config_.time_synchronization()) { + time_t t = time(NULL); + localtime_r(&t, ¤t_time); + current_time.tm_hour = current_time.tm_hour - 8; + packet_time_ns_ = 0; + gps_time = apollo::cyber::Time().Now().ToNanosecond(); + } else { + uint8_t *data = reinterpret_cast( + const_cast(packet->data().c_str())); + if (config_.packet_size() == 1212) { // 1212字节雷达 + if (0xff == data[1200]) { // ptp授时 + // std::cout << "ptp"; + basetime_ = (data[1201] * pow(2, 32) + data[1202] * pow(2, 24) + + data[1203] * pow(2, 16) + data[1204] * pow(2, 8) + + data[1205] * pow(2, 0)); + packet_time_ns_ = + (static_cast(data[1206]) + + static_cast(data[1207]) * pow(2, 8) + + static_cast(data[1208]) * pow(2, 16) + + static_cast(data[1209]) * pow(2, 24)); + } else { // gps授时 + current_time.tm_year = + static_cast(data[1200]) + 2000 - 1900; + current_time.tm_mon = static_cast(data[1201]) - 1; + current_time.tm_mday = static_cast(data[1202]); + current_time.tm_hour = static_cast(data[1203]); + current_time.tm_min = static_cast(data[1204]); + + if (config_.model() == LSLIDAR16P || + config_.model() == LSLIDAR32P) { + current_time.tm_sec = static_cast(data[1205]) + 1; + packet_time_ns_ = + (static_cast(data[1206]) + + static_cast(data[1207]) * pow(2, 8) + + static_cast(data[1208]) * pow(2, 16) + + static_cast(data[1209]) * pow(2, 24)) * + 1e3; // ns + } else if (config_.model() == LSLIDAR_C32_V4 || + config_.model() == LSLIDAR_C16_V4 || + config_.model() == LSLIDAR_C8_V4 || + config_.model() == LSLIDAR_C1_V4) { + current_time.tm_sec = static_cast(data[1205]); + packet_time_ns_ = + (static_cast(data[1206]) + + static_cast(data[1207]) * pow(2, 8) + + static_cast(data[1208]) * pow(2, 16) + + static_cast(data[1209]) * pow(2, 24)); + } + basetime_ = static_cast(timegm(¤t_time)); + } + } else { // 1206字节雷达，V3.0 + uint8_t *data = reinterpret_cast( + const_cast(packet->data().c_str())); + packet_time_ns_ = + (static_cast(data[1200]) + + static_cast(data[1201]) * pow(2, 8) + + static_cast(data[1202]) * pow(2, 16) + + static_cast(data[1203]) * pow(2, 24)) * + 1e3; // ns + basetime_ = static_cast(timegm(¤t_time)); + } + gps_time = basetime_ * 1000000000 + packet_time_ns_; + } + packet->set_stamp(gps_time); + break; + } else if (rc < 0) { + return rc; + } + } + packet->set_stamp(gps_time); // 設置包的時間 + + uint8_t *data = reinterpret_cast( + const_cast(packet->data().c_str())); + int azi1 = + 256 * static_cast(data[3]) + static_cast(data[2]); + int azi2 = 256 * static_cast(data[1103]) + + static_cast(data[1102]); + + if (((azi1 > 35000 && azi2 < 1000) || + (azi1 < 500 && i > config_.npackets() / 2))) { + scan_fill = true; + scan_start = *packet; + break; + } + i++; + } + } else if (config_.model() == LSLIDAR_LS128S2) { // LS128S2 + // Find the 0 degree packet, in order to fix the 0 degree angle position + scan_fill = false; + int i = 1; + bool is_found_frame_header = false; + while (scan->firing_pkts_size() < config_.npackets() && + !is_found_frame_header) { + LslidarPacket *packet = scan->add_firing_pkts(); + while (true) { + // keep reading until full packet received + int rc = input_->GetPacket(packet); + AINFO << "[debug ] line: " << __LINE__ << " file: " << __FILE__; + if (rc == 0) { + if (!config_.time_synchronization()) { + time_t t = time(NULL); + localtime_r(&t, ¤t_time); + current_time.tm_hour = current_time.tm_hour - 8; + gps_time = apollo::cyber::Time().Now().ToNanosecond(); + } else { + uint8_t *data = reinterpret_cast( + const_cast(packet->data().c_str())); + if (0xff == static_cast(data[1194])) { // ptp授时 + basetime_ = (static_cast(data[1195]) * 0 + + (static_cast(data[1196]) << 24) + + (static_cast(data[1197]) << 16) + + (static_cast(data[1198]) << 8) + + static_cast(data[1199]) * pow(2, 0)); + packet_time_ns_ = (static_cast(data[1200]) << 24) + + (static_cast(data[1201]) << 16) + + (static_cast(data[1202]) << 8) + + (static_cast(data[1203])); + gps_time = basetime_ * 1000000000 + packet_time_ns_; + } else { // gps授时 + struct tm cur_time {}; + memset(&cur_time, 0, sizeof(cur_time)); + cur_time.tm_sec = static_cast(data[1199]); + cur_time.tm_min = static_cast(data[1198]); + cur_time.tm_hour = static_cast(data[1197]); + cur_time.tm_mday = static_cast(data[1196]); + cur_time.tm_mon = static_cast(data[1195]) - 1; + cur_time.tm_year = + static_cast(data[1194]) + 2000 - 1900; + basetime_ = static_cast(timegm(&cur_time)); + packet_time_ns_ = + static_cast(data[1203]) + + (static_cast(data[1202]) << 8) + + (static_cast(data[1201]) << 16) + + (static_cast(data[1200]) << 24); // ns + gps_time = basetime_ * 1000000000 + packet_time_ns_; + } + } + break; + } else if (rc < 0) { + return rc; + } + } + packet->set_stamp(gps_time); // 設置包的時間 + uint8_t *data = reinterpret_cast( + const_cast(packet->data().c_str())); + int return_mode = static_cast(data[1205]); + + if (return_mode == 1) { + for (size_t point_idx = 0; point_idx < LS_POINTS_PER_PACKET_SINGLE_ECHO; + point_idx += 8) { // 一圈 + if ((static_cast(data[point_idx]) == 0xff) && + (static_cast(data[point_idx + 1]) == 0xaa) && + (static_cast(data[point_idx + 2]) == 0xbb) && + (static_cast(data[point_idx + 3]) == 0xcc) && + (static_cast(data[point_idx + 4]) == 0xdd)) { + scan_fill = false; + is_found_frame_header = true; + + break; + } + } + } else { + for (size_t point_idx = 0; point_idx < LS_POINTS_PER_PACKET_DOUBLE_ECHO; + point_idx += 12) { // 一圈 + if ((static_cast(data[point_idx]) == 0xff) && + (static_cast(data[point_idx + 1]) == 0xaa) && + (static_cast(data[point_idx + 2]) == 0xbb) && + (static_cast(data[point_idx + 3]) == 0xcc) && + (static_cast(data[point_idx + 4]) == 0xdd)) { + scan_fill = false; + is_found_frame_header = true; + AERROR << "\none circle! scan->firing_pkts_size(): " + << scan->firing_pkts_size(); + break; + } + } + } + i++; + } + } else { // ch系列 + // Find the 0 degree packet, in order to fix the 0 degree angle position + scan_fill = false; + int i = 1; + bool is_found_frame_header = false; + while (scan->firing_pkts_size() < config_.npackets() && + !is_found_frame_header) { + LslidarPacket *packet = scan->add_firing_pkts(); + while (true) { + // keep reading until full packet received + int rc = input_->GetPacket(packet); + AINFO << "[debug ] line: " << __LINE__ << " file: " << __FILE__; + if (rc == 0) { + if (config_.model() == LSLIDAR_CH64w || + config_.model() == LSLIDAR_CH120 || + config_.model() == LSLIDAR_CH128X1 || + config_.model() == LSLIDAR_CH32) { + if (!config_.time_synchronization()) { + time_t t = time(NULL); + localtime_r(&t, ¤t_time); + current_time.tm_hour = current_time.tm_hour - 8; + gps_time = apollo::cyber::Time().Now().ToNanosecond(); + } else { + uint8_t *data = reinterpret_cast( + const_cast(packet->data().c_str())); + current_time.tm_hour = static_cast(data[1197]); + current_time.tm_min = static_cast(data[1198]); + current_time.tm_sec = static_cast(data[1199]); + basetime_ = static_cast(timegm(¤t_time)); + if (time_service_mode == "gps") { + packet_time_ns_ = + (static_cast(data[1203]) + + static_cast(data[1202]) * pow(2, 8) + + static_cast(data[1201]) * pow(2, 16) + + static_cast(data[1200]) * pow(2, 24)) * + 1e3; // ns + } else if (time_service_mode == "gptp") { + packet_time_ns_ = + (static_cast(data[1203]) + + static_cast(data[1202]) * pow(2, 8) + + static_cast(data[1201]) * pow(2, 16) + + static_cast(data[1200]) * pow(2, 24)); // ns + } + gps_time = basetime_ * 1000000000 + packet_time_ns_; + } + } + break; + } else if (rc < 0) { + return rc; + } + } + packet->set_stamp(gps_time); // 設置包的時間 + uint8_t *data = reinterpret_cast( + const_cast(packet->data().c_str())); + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; + point_idx += 7) { // 一圈 + if ((static_cast(data[point_idx]) == 0xff) && + (static_cast(data[point_idx + 1]) == 0xaa) && + (static_cast(data[point_idx + 2]) == 0xbb) && + (static_cast(data[point_idx + 3]) == 0xcc)) { + scan_fill = false; + is_found_frame_header = true; + AERROR << "\none circle! scan->firing_pkts_size(): " + << scan->firing_pkts_size(); + break; + } + } + i++; + } + } + return 0; +} + +void LslidarDriver::difopPoll(void) { + while (!cyber::IsShutdown()) { + std::shared_ptr scans = + std::make_shared(); + std::shared_ptr packet = std::make_shared(); + while (!cyber::IsShutdown()) { + int rc = positioning_input_->GetPacket(packet.get()); + if (rc == 0) { + break; // got a full packet + } + if (rc < 0) { + return; + } + } + uint8_t *data = + reinterpret_cast(const_cast(packet->data().c_str())); + { + std::unique_lock lock(mutex_); + memcpy(bytes, data, FIRING_DATA_PACKET_SIZE); + } + if (!config_.time_synchronization()) { + time_t t = time(NULL); + localtime_r(&t, ¤t_time); + current_time.tm_hour = current_time.tm_hour - 8; + } else { + if (data[0] == 0xA5 && data[1] == 0xFF && data[2] == 0x00 && + data[3] == 0x5A) { + if (config_.model() == LSLIDAR_CH16 || + config_.model() == LSLIDAR_CH32 || + config_.model() == LSLIDAR_CH128 || + config_.model() == LSLIDAR_CH64) { + current_time.tm_year = static_cast(data[36] + 100); + current_time.tm_mon = static_cast(data[37] - 1); + current_time.tm_mday = static_cast(data[38]); + current_time.tm_hour = static_cast(data[39]); + current_time.tm_min = static_cast(data[40]); + current_time.tm_sec = static_cast(data[41]); + } else if (config_.model() == LSLIDAR_CH64w) { + current_time.tm_year = static_cast(data[52] + 100); + current_time.tm_mon = static_cast(data[53] - 1); + current_time.tm_mday = static_cast(data[54]); + if (data[44] == 0x00) { // gps授时 + time_service_mode = "gps"; + } else if (data[44] == 0x01) { // ptp授时 + time_service_mode = "gptp"; + } + } else if (config_.model() == LSLIDAR_CH120) { + current_time.tm_year = static_cast(data[36] + 100); + current_time.tm_mon = static_cast(data[37] - 1); + current_time.tm_mday = static_cast(data[38]); + } else if (config_.model() == LSLIDAR_CH128X1) { + current_time.tm_year = static_cast(data[52] + 100); + current_time.tm_mon = static_cast(data[53] - 1); + current_time.tm_mday = static_cast(data[54]); + if (data[44] == 0x00) { // gps授时 + time_service_mode = "gps"; + } else if (data[44] == 0x01) { // ptp授时 + time_service_mode = "gptp"; + } + } else if (config_.packet_size() == 1206 && + (config_.model() == LSLIDAR32P || + config_.model() == LSLIDAR16P)) { + current_time.tm_year = static_cast(data[52] + 100); + current_time.tm_mon = static_cast(data[53] - 1); + current_time.tm_mday = static_cast(data[54]); + current_time.tm_hour = static_cast(data[55]); + current_time.tm_min = static_cast(data[56]); + current_time.tm_sec = static_cast(data[57]); + } + } + } + } +} + +LslidarDriver *LslidarDriverFactory::CreateDriver(const Config &config) { + LslidarDriver *driver = nullptr; + driver = new LslidarDriver(config); + return driver; +} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/driver/driver.h b/modules/drivers/lidar/lslidar/driver/driver.h new file mode 100755 index 00000000000..2badd457b01 --- /dev/null +++ b/modules/drivers/lidar/lslidar/driver/driver.h @@ -0,0 +1,97 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "modules/drivers/lidar/lslidar/proto/config.pb.h" +#include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" + +#include "cyber/cyber.h" +#include "modules/drivers/lidar/lslidar/driver/input.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +constexpr int BLOCKS_PER_PACKET = 12; +constexpr int BLOCK_SIZE = 100; + +static const unsigned int POINTS_ONE_CHANNEL_PER_SECOND = 20000; +static const unsigned int BLOCKS_ONE_CHANNEL_PER_PKT = 12; +static const int POINTS_PER_PACKET = 171 * 7; // ch系列 +static const int LS_POINTS_PER_PACKET_SINGLE_ECHO = + 149 * 8; // LS 1550nm系列 单回波 +static const int LS_POINTS_PER_PACKET_DOUBLE_ECHO = + 99 * 12; // LS 1550nm系列 单回波 + +class LslidarDriver { + public: + explicit LslidarDriver(const Config &config) : config_(config) { + // scan_start = new LslidarPacket(); + } + ~LslidarDriver(); + + bool Poll(const std::shared_ptr &scan); + void Init(); + void difopPoll(); + void SetPacketRate(const double packet_rate) { packet_rate_ = packet_rate; } + int npackets; + struct tm current_time; + + protected: + Config config_; + std::unique_ptr input_ = nullptr; + std::unique_ptr positioning_input_ = nullptr; + std::string topic_; + double packet_rate_ = 0.0; + bool scan_fill = false; + uint64_t gps_time = 0; + uint64_t last_gps_time = 0; + + uint64_t basetime_ = 0; + uint64_t packet_time_ns_ = 0; + + uint32_t last_gps_time_ = 0; + uint64_t last_count_ = 0; + static uint64_t sync_counter; + + std::thread difop_thread_; + int PollStandard(std::shared_ptr scan); + LslidarPacket scan_start; + LslidarPacket last_scan_start; + + LslidarPacket scan_start1; + LslidarPacket scan_start2; + std::mutex mutex_; + uint8_t bytes[FIRING_DATA_PACKET_SIZE] = {0x00}; + std::string time_service_mode = {"gps"}; +}; + +class LslidarDriverFactory { + public: + static LslidarDriver *CreateDriver(const Config &config); +}; + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/driver/input.cc b/modules/drivers/lidar/lslidar/driver/input.cc new file mode 100755 index 00000000000..3c37dc9680e --- /dev/null +++ b/modules/drivers/lidar/lslidar/driver/input.cc @@ -0,0 +1,234 @@ +/****************************************************************************** + * Copyright 2020 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/driver/input.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace apollo { +namespace drivers { +namespace lslidar { + +Input::Input(uint16_t port, std::string lidar_ip, int packet_size) { + packet_size_ = packet_size; + inet_aton(lidar_ip.c_str(), &devip_); +} + +int Input::GetPacket(LslidarPacket *pkt) { return 0; } + +Input::~Input(void) { (void)close(sockfd_); } + +InputSocket::InputSocket(uint16_t port, std::string lidar_ip, int packet_size) + : Input(port, lidar_ip, packet_size) { + port_ = port; + sockfd_ = -1; + sockfd_ = socket(PF_INET, SOCK_DGRAM, 0); + if (-1 == sockfd_) { + AERROR << "socket open error"; + return; + } + + inet_aton(lidar_ip.c_str(), &devip_); + sockaddr_in myAddress; // my address information + memset(&myAddress, 0, sizeof(myAddress)); // initialize to zeros + myAddress.sin_family = AF_INET; // host byte order + myAddress.sin_port = htons(port); // port in network byte order + myAddress.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP + + if (bind(sockfd_, reinterpret_cast(&myAddress), + sizeof(sockaddr)) == -1) { + AERROR << "bind error, port:" << port; + return; + } + + if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0) { + AERROR << "fcntl error"; + return; + } +} + +InputSocket::~InputSocket(void) { (void)close(sockfd_); } + +int InputSocket::GetPacket(LslidarPacket *pkt) { + double time1 = apollo::cyber::Time().Now().ToSecond(); + struct pollfd fds[1]; + fds[0].fd = sockfd_; + fds[0].events = POLLIN; + static const int POLL_TIMEOUT = 3000; // one second (in msec) + + sockaddr_in sender_address; + socklen_t sender_address_len = sizeof(sender_address); + + while (true) { + // Receive packets that should now be available from the + // socket using a blocking read. + // poll() until input available + do { + int retval = poll(fds, 1, POLL_TIMEOUT); + if (retval < 0) { + if (errno != EINTR) AERROR << "poll() error: " << strerror(errno); + + return 1; + } + if (retval == 0) { + AERROR << "lslidar poll() timeout, port: " << port_; + return 1; + } + if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || + (fds[0].revents & POLLNVAL)) { + AWARN << "poll() reports lslidar error"; + return 1; + } + } while ((fds[0].revents & POLLIN) == 0); + uint8_t bytes[1212]; + ssize_t nbytes = recvfrom(sockfd_, bytes, packet_size_, 0, + reinterpret_cast(&sender_address), + &sender_address_len); + if (nbytes < 0) { + if (errno != EWOULDBLOCK) { + AERROR << "recvfail"; + return 1; + } + } else if ((size_t)nbytes == size_t(packet_size_)) { + if (sender_address.sin_addr.s_addr != devip_.s_addr) { + AERROR << "lidar IP parameter set error,please reset in config file"; + continue; + } else { + pkt->set_data(bytes, packet_size_); + break; + } + } + AERROR << "incomplete lslidar packet read: " << nbytes << " bytes"; + } + + // Average the times at which we begin and end reading. Use that to + // estimate when the scan occurred. + double time2 = apollo::cyber::Time().Now().ToSecond(); + AINFO << apollo::cyber::Time((time2 + time1) / 2.0).ToNanosecond(); + return 0; +} + +InputPCAP::InputPCAP(uint16_t port, std::string lidar_ip, int packet_size, + double packet_rate, std::string filename, bool read_once, + bool read_fast, double repeat_delay) + : Input(port, lidar_ip, packet_size), + packet_rate_(packet_rate), + filename_(filename) { + pcap_ = NULL; + empty_ = true; + packet_rate_ = packet_rate; + lidar_ip_ = lidar_ip; + // get parameters using private node handle + read_once_ = read_once; + read_fast_ = read_fast; + repeat_delay_ = repeat_delay; + + if (read_once_) AINFO << "Read input file only once."; + if (read_fast_) AINFO << "Read input file as quickly as possible."; + if (repeat_delay_ > 0.0) + AINFO << "Delay %.3f seconds before repeating input file." << repeat_delay_; + + // Open the PCAP dump file + AERROR << "Opening PCAP file " << filename_; + if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL) { + AERROR << "Error opening lslidar socket dump file."; + return; + } + + std::stringstream filter; + if (lidar_ip != "") { + filter << "src host " << lidar_ip << " && "; + } + filter << "udp dst port " << port; + pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, + PCAP_NETMASK_UNKNOWN); +} + +/** destructor */ +InputPCAP::~InputPCAP(void) { pcap_close(pcap_); } + +/** @brief Get one lslidar packet. */ +int InputPCAP::GetPacket(LslidarPacket *pkt) { + struct pcap_pkthdr *header; + const u_char *pkt_data; + + // while (flag == 1) + while (!apollo::cyber::IsShutdown()) { + int res; + if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0) { + // Skip packets not for the correct port and from the + // selected IP address. + if (!lidar_ip_.empty() && + (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data))) + continue; + + // Keep the reader from blowing through the file. + if (read_fast_ == false) + usleep(static_cast(1000 * 1000 / packet_rate_ / 1.1)); + if (1206 == packet_size_) { + uint8_t bytes[1206]; + memcpy(bytes, pkt_data + 42, packet_size_); + pkt->set_data(bytes, packet_size_); + } else if (1212 == packet_size_) { + uint8_t bytes[1212]; + memcpy(bytes, pkt_data + 42, packet_size_); + pkt->set_data(bytes, packet_size_); + } + + empty_ = false; + return 0; // success + } + + if (empty_) { + AINFO << "Error " << res + << " reading lslidar packet: " << pcap_geterr(pcap_); + return -1; + } + + if (read_once_) { + AINFO << "end of file reached -- done reading."; + return -1; + } + + if (repeat_delay_ > 0.0) { + AINFO << "end of file reached -- delaying" << repeat_delay_ << "seconds."; + usleep(rint(repeat_delay_ * 1000000.0)); + } + + AINFO << "replaying lslidar dump file"; + + // I can't figure out how to rewind the file, because it + // starts with some kind of header. So, close the file + // and reopen it with pcap. + pcap_close(pcap_); + pcap_ = pcap_open_offline(filename_.c_str(), errbuf_); + empty_ = true; // maybe the file disappeared? + } // loop back and try again + return 0; +} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/driver/input.h b/modules/drivers/lidar/lslidar/driver/input.h new file mode 100755 index 00000000000..99c87f196be --- /dev/null +++ b/modules/drivers/lidar/lslidar/driver/input.h @@ -0,0 +1,94 @@ +/****************************************************************************** + * Copyright 2020 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include + + +#include +#include +#include +#include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" + +#include "cyber/cyber.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +static const size_t FIRING_DATA_PACKET_SIZE = 1212; +static uint16_t MSOP_DATA_PORT_NUMBER = + 2368; // lslidar default data port on PC + +class Input { + public: + Input(uint16_t portport = MSOP_DATA_PORT_NUMBER, + std::string lidar_ip = "192.168.1.200", int packet_size = 1212); + virtual ~Input(); + virtual int GetPacket(LslidarPacket *pkt); + + protected: + int port_; + int sockfd_; + uint64_t pointcloudTimeStamp; + in_addr devip_; + int packet_size_; +}; + +/** @brief Live lslidar input from socket. */ +class InputSocket : public Input { + public: + InputSocket(uint16_t port = MSOP_DATA_PORT_NUMBER, + std::string lidar_ip = "192.168.1.200", int packet_size = 1212); + + virtual ~InputSocket(); + + virtual int GetPacket(LslidarPacket *pkt); +}; + +/** @brief lslidar input from PCAP dump file. + * + * Dump files can be grabbed by libpcap + */ +class InputPCAP : public Input { + public: + InputPCAP(uint16_t port = MSOP_DATA_PORT_NUMBER, + std::string lidar_ip = "192.168.1.200", int packet_size = 1212, + double packet_rate = 0.0, std::string filename = "", + bool read_once = false, bool read_fast = false, + double repeat_delay = 0.0); + + virtual ~InputPCAP(); + + virtual int GetPacket(LslidarPacket *pkt); + + private: + double packet_rate_; + std::string filename_; + pcap_t *pcap_; + bpf_program pcap_packet_filter_; + char errbuf_[PCAP_ERRBUF_SIZE]; + bool empty_; + bool read_once_; + bool read_fast_; + double repeat_delay_; + std::string lidar_ip_; +}; + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/driver/lslidar_driver_component.cc b/modules/drivers/lidar/lslidar/driver/lslidar_driver_component.cc new file mode 100755 index 00000000000..612a2369367 --- /dev/null +++ b/modules/drivers/lidar/lslidar/driver/lslidar_driver_component.cc @@ -0,0 +1,79 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/driver/lslidar_driver_component.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +static void my_handler(int sig) { exit(-1); } + +bool LslidarDriverComponent::Init() { + signal(SIGINT, my_handler); + AINFO << "Lslidar driver component init"; + Config lslidar_config; + if (!GetProtoConfig(&lslidar_config)) { + return false; + } + + // start the driver + writer_ = node_->CreateWriter( + lslidar_config.scan_channel()); + LslidarDriver *driver = LslidarDriverFactory::CreateDriver(lslidar_config); + if (driver == nullptr) { + return false; + } + dvr_.reset(driver); + dvr_->Init(); + // spawn device poll thread + runing_ = true; + device_thread_ = std::shared_ptr( + new std::thread(std::bind(&LslidarDriverComponent::device_poll, this))); + device_thread_->detach(); + + return true; +} + +/** @brief Device poll thread main loop. */ +void LslidarDriverComponent::device_poll() { + signal(SIGINT, my_handler); + while (!apollo::cyber::IsShutdown()) { + // poll device until end of file + std::shared_ptr scan = + std::make_shared(); + + bool ret = dvr_->Poll(scan); + if (ret) { + common::util::FillHeader("lslidar", scan.get()); + AINFO << "publish scan!"; + double time1 = apollo::cyber::Time().Now().ToSecond(); + writer_->Write(scan); + double time2 = apollo::cyber::Time().Now().ToSecond(); + AINFO << "apollo::cyber::Time((time2 - time1)" + << apollo::cyber::Time((time2 - time1) / 2.0).ToNanosecond(); + } else { + AWARN << "device poll failed"; + } + } + + AERROR << "CompLslidarDriver thread exit"; + runing_ = false; +} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/driver/lslidar_driver_component.h b/modules/drivers/lidar/lslidar/driver/lslidar_driver_component.h new file mode 100755 index 00000000000..8648104fe86 --- /dev/null +++ b/modules/drivers/lidar/lslidar/driver/lslidar_driver_component.h @@ -0,0 +1,60 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ +#pragma once + +#include +#include +#include +#include "modules/drivers/lidar/lslidar/proto/config.pb.h" +#include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" + +#include "cyber/cyber.h" +#include "modules/common/util/message_util.h" +#include "modules/drivers/lidar/lslidar/driver/driver.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +using apollo::cyber::Component; +using apollo::cyber::Reader; +using apollo::cyber::Writer; +using apollo::drivers::lslidar::LslidarScan; + +class LslidarDriverComponent : public Component<> { + public: + ~LslidarDriverComponent() { + if (device_thread_->joinable()) { + device_thread_->join(); + } + } + bool Init() override; + + private: + void device_poll(); + volatile bool runing_; ///< device thread is running + uint32_t seq_ = 0; + std::shared_ptr device_thread_; + std::shared_ptr dvr_; ///< driver implementation class + std::shared_ptr> + writer_; +}; + +CYBER_REGISTER_COMPONENT(LslidarDriverComponent) + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/launch/lslidar16.launch b/modules/drivers/lidar/lslidar/launch/lslidar16.launch new file mode 100644 index 00000000000..363bcca53e1 --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidar16.launch @@ -0,0 +1,7 @@ + + + lslidar16 + /apollo/modules/drivers/lidar/lslidar/dag/lslidar16.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidar16_32_128X1.launch b/modules/drivers/lidar/lslidar/launch/lslidar16_32_128X1.launch new file mode 100644 index 00000000000..8996d283f8e --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidar16_32_128X1.launch @@ -0,0 +1,7 @@ + + + lslidar16_32_128X1 + /apollo/modules/drivers/lidar/lslidar/dag/lslidar16_32_128X1.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidar16_32_32w.launch b/modules/drivers/lidar/lslidar/launch/lslidar16_32_32w.launch new file mode 100644 index 00000000000..7e86ea49d0a --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidar16_32_32w.launch @@ -0,0 +1,7 @@ + + + lslidar16_32_32w + /apollo/modules/drivers/lidar/lslidar/dag/lslidar16_32_32w.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidar16_32_64w.launch b/modules/drivers/lidar/lslidar/launch/lslidar16_32_64w.launch new file mode 100644 index 00000000000..5b53b0d97c2 --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidar16_32_64w.launch @@ -0,0 +1,7 @@ + + + lslidar16_32_64w + /apollo/modules/drivers/lidar/lslidar/dag/lslidar16_32_64w.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidar16_32_64w_128X1.launch b/modules/drivers/lidar/lslidar/launch/lslidar16_32_64w_128X1.launch new file mode 100644 index 00000000000..cd1618c49b0 --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidar16_32_64w_128X1.launch @@ -0,0 +1,7 @@ + + + lslidar16_32_64w_ch128X1 + /apollo/modules/drivers/lidar/lslidar/dag/lslidar16_32_64w_128X1.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidar16v4.launch b/modules/drivers/lidar/lslidar/launch/lslidar16v4.launch new file mode 100644 index 00000000000..798e33fe6c6 --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidar16v4.launch @@ -0,0 +1,7 @@ + + + lslidar16v4 + /apollo/modules/drivers/lidar/lslidar/dag/lslidar16v4.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidar1v4.launch b/modules/drivers/lidar/lslidar/launch/lslidar1v4.launch new file mode 100644 index 00000000000..5272bcfef1a --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidar1v4.launch @@ -0,0 +1,7 @@ + + + lslidar1v4 + /apollo/modules/drivers/lidar/lslidar/dag/lslidar1v4.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidar32.launch b/modules/drivers/lidar/lslidar/launch/lslidar32.launch new file mode 100644 index 00000000000..3ab784f6422 --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidar32.launch @@ -0,0 +1,7 @@ + + + lslidar32 + /apollo/modules/drivers/lidar/lslidar/dag/lslidar32.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidar32v4.launch b/modules/drivers/lidar/lslidar/launch/lslidar32v4.launch new file mode 100644 index 00000000000..f222f4bb5de --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidar32v4.launch @@ -0,0 +1,7 @@ + + + lslidar32v4 + /apollo/modules/drivers/lidar/lslidar/dag/lslidar32v4.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidar401.launch b/modules/drivers/lidar/lslidar/launch/lslidar401.launch new file mode 100644 index 00000000000..002a736870a --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidar401.launch @@ -0,0 +1,7 @@ + + + lslidar401 + /apollo/modules/drivers/lidar/lslidar/dag/lslidar401.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidar8v4.launch b/modules/drivers/lidar/lslidar/launch/lslidar8v4.launch new file mode 100644 index 00000000000..41300b7b66d --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidar8v4.launch @@ -0,0 +1,7 @@ + + + lslidar8v4 + /apollo/modules/drivers/lidar/lslidar/dag/lslidar8v4.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidarCH120.launch b/modules/drivers/lidar/lslidar/launch/lslidarCH120.launch new file mode 100644 index 00000000000..94200c526fd --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidarCH120.launch @@ -0,0 +1,7 @@ + + + lslidarCH120 + /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH120.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidarCH128.launch b/modules/drivers/lidar/lslidar/launch/lslidarCH128.launch new file mode 100644 index 00000000000..8d6eed0cf05 --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidarCH128.launch @@ -0,0 +1,7 @@ + + + lslidarCH128 + /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH128.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidarCH128X1.launch b/modules/drivers/lidar/lslidar/launch/lslidarCH128X1.launch new file mode 100644 index 00000000000..b6d952ee5a6 --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidarCH128X1.launch @@ -0,0 +1,7 @@ + + + lslidarCH128X1 + /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH128X1.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidarCH16.launch b/modules/drivers/lidar/lslidar/launch/lslidarCH16.launch new file mode 100644 index 00000000000..3311d28a629 --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidarCH16.launch @@ -0,0 +1,7 @@ + + + lslidarCH16 + /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH16.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidarCH32.launch b/modules/drivers/lidar/lslidar/launch/lslidarCH32.launch new file mode 100644 index 00000000000..268ffc9aa0d --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidarCH32.launch @@ -0,0 +1,7 @@ + + + lslidarCH32 + /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH32.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidarCH64.launch b/modules/drivers/lidar/lslidar/launch/lslidarCH64.launch new file mode 100644 index 00000000000..45f85aa434c --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidarCH64.launch @@ -0,0 +1,7 @@ + + + lslidarCH64 + /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH64.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidarCH64w.launch b/modules/drivers/lidar/lslidar/launch/lslidarCH64w.launch new file mode 100644 index 00000000000..e94b56a8df4 --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidarCH64w.launch @@ -0,0 +1,7 @@ + + + lslidarCH64w + /apollo/modules/drivers/lidar/lslidar/dag/lslidarCH64w.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/launch/lslidarLS128S2.launch b/modules/drivers/lidar/lslidar/launch/lslidarLS128S2.launch new file mode 100644 index 00000000000..ff4d51c805a --- /dev/null +++ b/modules/drivers/lidar/lslidar/launch/lslidarLS128S2.launch @@ -0,0 +1,7 @@ + + + lslidarLS128S2 + /apollo/modules/drivers/lidar/lslidar/dag/lslidarLS128S2.dag + lslidar + + diff --git a/modules/drivers/lidar/lslidar/params/LS16_calibration.yaml b/modules/drivers/lidar/lslidar/params/LS16_calibration.yaml new file mode 100644 index 00000000000..bb6212576f2 --- /dev/null +++ b/modules/drivers/lidar/lslidar/params/LS16_calibration.yaml @@ -0,0 +1,51 @@ +lasers: +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, rot_correction: 0.0, + vert_correction: -0.2617993877991494, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, rot_correction: 0.0, + vert_correction: 0.017453292519943295, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, rot_correction: 0.0, + vert_correction: -0.22689280275926285, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, rot_correction: 0.0, + vert_correction: 0.05235987755982989, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, rot_correction: 0.0, + vert_correction: -0.19198621771937624, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, rot_correction: 0.0, + vert_correction: 0.08726646259971647, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, rot_correction: 0.0, + vert_correction: -0.15707963267948966, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, rot_correction: 0.0, + vert_correction: 0.12217304763960307, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, rot_correction: 0.0, + vert_correction: -0.12217304763960307, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, rot_correction: 0.0, + vert_correction: 0.15707963267948966, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, rot_correction: 0.0, + vert_correction: -0.08726646259971647, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, rot_correction: 0.0, + vert_correction: 0.19198621771937624, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, rot_correction: 0.0, + vert_correction: -0.05235987755982989, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, rot_correction: 0.0, + vert_correction: 0.22689280275926285, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, rot_correction: 0.0, + vert_correction: -0.017453292519943295, vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, rot_correction: 0.0, + vert_correction: 0.2617993877991494, vert_offset_correction: 0.0} +num_lasers: 16 + diff --git a/modules/drivers/lidar/lslidar/params/LS32_calibration.yaml b/modules/drivers/lidar/lslidar/params/LS32_calibration.yaml new file mode 100644 index 00000000000..e3e5c67cc4d --- /dev/null +++ b/modules/drivers/lidar/lslidar/params/LS32_calibration.yaml @@ -0,0 +1,131 @@ +lasers: +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.27925268031909273, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.26179938779914944, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.24434609527920614, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.22689280275926285, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.20943951023931955, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.19198621771937625, +vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.17453292519943296, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.15707963267948966, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.13962634015954637, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.12217304763960307, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.10471975511965977, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.08726646259971647, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.06981317007977318, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.05235987755982989, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.03490658503988659, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.01745329251994329, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 16, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 17, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.01745329251994329, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 18, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.03490658503988659, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 19, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.05235987755982989, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 20, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.06981317007977318, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 21, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.08726646259971647, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 22, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.10471975511965977, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 23, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.12217304763960307, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 24, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.13962634015954637, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 25, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.15707963267948966, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 26, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.17453292519943296, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 27, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.19198621771937625, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 28, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.20943951023931955, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 29, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.22689280275926285, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 30, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.24434609527920614, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 31, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.26179938779914944, + vert_offset_correction: 0.0} + +num_lasers: 32 diff --git a/modules/drivers/lidar/lslidar/params/LSCH120_calibration.yaml b/modules/drivers/lidar/lslidar/params/LSCH120_calibration.yaml new file mode 100644 index 00000000000..6368b753f47 --- /dev/null +++ b/modules/drivers/lidar/lslidar/params/LSCH120_calibration.yaml @@ -0,0 +1,482 @@ +lasers: +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.22689280276, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.22397810290916, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.22106340305832, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.21814870320748, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.21523400335664, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.2123193035058, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.20940460365496, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.20648990380412, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.20357520395328, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.20066050410244, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.1977458042516, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.19483110440076, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.19191640454992, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.18900170469908, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.18608700484824, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.1831723049974, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 16, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.18025760514656, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 17, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.17734290529572, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 18, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.17442820544488, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 19, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.17151350559404, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 20, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.1685988057432, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 21, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.16568410589236, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 22, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.16276940604152, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 23, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.15985470619068, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 24, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.15694000633984, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 25, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.154025306489, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 26, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.15111060663816, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 27, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.14819590678732, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 28, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.14528120693648, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 29, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.14236650708564, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 30, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.1394518072348, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 31, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.13653710738396, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 32, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.13362240753312, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 33, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.13070770768228, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 34, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.12779300783144, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 35, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.1248783079806, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 36, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.12196360812976, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 37, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.11904890827892, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 38, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.11613420942808, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 39, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.11321950857724, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 40, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.1103048087264, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 41, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.10739010887556, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 42, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.10447540902472, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 43, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.10156070917388, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 44, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.09864600932304, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 45, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.0957313094722, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 46, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.09281660962136, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 47, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.08990190977052, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 48, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.08698720991968, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 49, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.08407251006884, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 50, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.081157810218, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 51, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.07824311036716, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 52, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.07532841051632, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 53, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.07241371066548, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 54, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.06949901081464, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 55, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.0665843109638, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 56, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.06366961111296, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 57, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.06075491126212, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 58, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.05784021141128, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 59, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.05492551156044, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 60, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.0520108117096, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 61, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.04909611185876, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 62, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.04618141200792, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 63, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.04326671215708, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 64, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.04035201230624, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 65, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.0374373124554, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 66, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.03452261260456, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 67, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.03160791275372, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 68, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.02869321290288, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 69, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.02577851305204, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 70, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.0228638132012, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 71, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.01994911335036, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 72, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.01703441349952, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 73, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.01411971364868, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 74, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.01120501379784, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 75, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.008290313947, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 76, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: -0.00537561409616, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 77, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: -0.00246091424532, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 78, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.00045378560552, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 79, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.00336848545636, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 80, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0062831853072, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 81, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.00919788515804, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 82, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.01211258500888, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 83, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.01502728485972, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 84, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.01794198471056, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 85, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0208566845614, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 86, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.02377138441224, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 87, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.02668608426308, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 88, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.02960078411392, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 89, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.03251548396476, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 90, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0354301838156, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 91, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.03834488366644, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 92, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.04125958351728, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 93, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.04417428336812, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 94, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.04708898321896, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 95, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0500036830698, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 96, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.05291838292064, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 97, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.05583308277148, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 98, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.05874778262232, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 99, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.06166248247316, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 100, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.064577182324, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 101, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.06749188217484, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 102, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.07040658202568, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 103, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.07332128187652, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 104, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.07623598172736, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 105, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0791506815782, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 106, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.08206538142904, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 107, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.08498008127988, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 108, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.08789478113072, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 109, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.09080948098156, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 110, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0937241808324, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 111, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.09663888068324, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 112, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.09955358053408, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 113, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.10246828038492, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 114, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.10538298023576, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 115, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.1082976800866, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 116, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.11121237993744, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 117, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.11412707978828, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 118, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.11704177963912, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 119, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.11995647948996, + vert_offset_correction: 0.0} +num_lasers: 120 diff --git a/modules/drivers/lidar/lslidar/params/LSCH128X1_calibration.yaml b/modules/drivers/lidar/lslidar/params/LSCH128X1_calibration.yaml new file mode 100644 index 00000000000..582e9ad1d32 --- /dev/null +++ b/modules/drivers/lidar/lslidar/params/LSCH128X1_calibration.yaml @@ -0,0 +1,514 @@ +lasers: +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 16, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 17, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 18, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 19, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 20, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 21, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 22, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 23, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 24, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 25, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 26, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 27, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 28, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 29, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 30, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 31, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 32, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 33, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 34, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 35, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 36, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 37, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, +vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 38, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 39, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 40, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 41, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 42, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 43, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 44, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 45, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 46, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 47, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 48, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 49, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 50, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 51, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 52, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 53, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 54, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 55, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 56, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 57, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 58, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 59, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 60, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 61, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 62, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 63, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 64, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 65, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 66, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 67, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 68, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 69, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, +vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 70, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 71, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 72, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 73, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 74, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 75, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 76, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 77, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 78, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 79, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 80, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 81, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 82, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 83, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 84, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 85, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 86, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 87, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 88, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 89, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 90, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 91, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 92, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 93, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 94, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 95, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 96, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 97, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 98, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 99, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 100, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 101, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, +vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 102, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 103, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 104, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 105, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 106, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 107, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 108, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 109, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 110, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 111, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 112, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 113, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 114, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 115, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 116, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 117, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 118, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 119, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 120, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 121, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 122, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 123, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 124, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 125, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 126, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 127, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +num_lasers: 128 \ No newline at end of file diff --git a/modules/drivers/lidar/lslidar/params/LSCH128_calibration.yaml b/modules/drivers/lidar/lslidar/params/LSCH128_calibration.yaml new file mode 100644 index 00000000000..582e9ad1d32 --- /dev/null +++ b/modules/drivers/lidar/lslidar/params/LSCH128_calibration.yaml @@ -0,0 +1,514 @@ +lasers: +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 16, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 17, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 18, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 19, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 20, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 21, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 22, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 23, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 24, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 25, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 26, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 27, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 28, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 29, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 30, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 31, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 32, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 33, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 34, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 35, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 36, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 37, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, +vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 38, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 39, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 40, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 41, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 42, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 43, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 44, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 45, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 46, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 47, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 48, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 49, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 50, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 51, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 52, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 53, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 54, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 55, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 56, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 57, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 58, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 59, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 60, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 61, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 62, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 63, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 64, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 65, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 66, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 67, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 68, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 69, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, +vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 70, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 71, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 72, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 73, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 74, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 75, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 76, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 77, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 78, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 79, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 80, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 81, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 82, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 83, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 84, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 85, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 86, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 87, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 88, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 89, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 90, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 91, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 92, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 93, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 94, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 95, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 96, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 97, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 98, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 99, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 100, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 101, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, +vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 102, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 103, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 104, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 105, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 106, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 107, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 108, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 109, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 110, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 111, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 112, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 113, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 114, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 115, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 116, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 117, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 118, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 119, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 120, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 121, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 122, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 123, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 124, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 125, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 126, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 127, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +num_lasers: 128 \ No newline at end of file diff --git a/modules/drivers/lidar/lslidar/params/LSCH16_calibration.yaml b/modules/drivers/lidar/lslidar/params/LSCH16_calibration.yaml new file mode 100644 index 00000000000..840175a94f3 --- /dev/null +++ b/modules/drivers/lidar/lslidar/params/LSCH16_calibration.yaml @@ -0,0 +1,67 @@ +lasers: +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, +vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} + +num_lasers: 16 diff --git a/modules/drivers/lidar/lslidar/params/LSCH32_calibration.yaml b/modules/drivers/lidar/lslidar/params/LSCH32_calibration.yaml new file mode 100644 index 00000000000..09c1edcbfa4 --- /dev/null +++ b/modules/drivers/lidar/lslidar/params/LSCH32_calibration.yaml @@ -0,0 +1,131 @@ +lasers: +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, +vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 16, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 17, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 18, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 19, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 20, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 21, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 22, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 23, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 24, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 25, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 26, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 27, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 28, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 29, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 30, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 31, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} + +num_lasers: 32 diff --git a/modules/drivers/lidar/lslidar/params/LSCH64_calibration.yaml b/modules/drivers/lidar/lslidar/params/LSCH64_calibration.yaml new file mode 100644 index 00000000000..7de00b6b34e --- /dev/null +++ b/modules/drivers/lidar/lslidar/params/LSCH64_calibration.yaml @@ -0,0 +1,258 @@ +lasers: +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, +vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 16, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 17, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 18, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 19, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 20, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 21, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 22, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 23, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 24, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 25, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 26, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 27, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 28, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 29, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 30, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 31, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 32, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 33, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 34, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 35, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 36, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 37, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, +vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 38, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 39, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 40, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 41, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 42, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 43, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 44, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 45, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 46, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 47, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 48, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 49, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 50, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 51, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 52, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 53, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 54, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 55, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 56, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 57, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 58, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 59, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 60, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 61, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 62, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 63, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +num_lasers: 64 \ No newline at end of file diff --git a/modules/drivers/lidar/lslidar/params/LSCH64w_calibration.yaml b/modules/drivers/lidar/lslidar/params/LSCH64w_calibration.yaml new file mode 100644 index 00000000000..7de00b6b34e --- /dev/null +++ b/modules/drivers/lidar/lslidar/params/LSCH64w_calibration.yaml @@ -0,0 +1,258 @@ +lasers: +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, +vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 16, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 17, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 18, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 19, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 20, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 21, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 22, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 23, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 24, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 25, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 26, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 27, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 28, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 29, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 30, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 31, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 32, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 33, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 34, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 35, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 36, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 37, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, +vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 38, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 39, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 40, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 41, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 42, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 43, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 44, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 45, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 46, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 47, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 48, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 49, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 50, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 51, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 52, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 53, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 54, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 55, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 56, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 57, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 58, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 59, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 60, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 61, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 62, max_intensity: 255, + min_intensity: 0, rot_correction: -0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0, + focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 63, max_intensity: 255, + min_intensity: 0, rot_correction: 0.0, vert_correction: 0.0, + vert_offset_correction: 0.0} +num_lasers: 64 \ No newline at end of file diff --git a/modules/drivers/lidar/lslidar/params/ls16_novatel_extrinsics.yaml b/modules/drivers/lidar/lslidar/params/ls16_novatel_extrinsics.yaml new file mode 100644 index 00000000000..13d47a8b672 --- /dev/null +++ b/modules/drivers/lidar/lslidar/params/ls16_novatel_extrinsics.yaml @@ -0,0 +1,17 @@ +header: + seq: 0 + stamp: + secs: 1598864145 + nsecs: 0 + frame_id: novatel +transform: + rotation: + x: -0.010 + y: -0.007 + z: 0.174 + w: 0.985 + translation: + x: -0.521985649500604 + y: 0.6272410563007734 + z: 0.2723201225767891 +child_frame_id: lslidar16 diff --git a/modules/drivers/lidar/lslidar/params/ls32_novatel_extrinsics.yaml b/modules/drivers/lidar/lslidar/params/ls32_novatel_extrinsics.yaml new file mode 100644 index 00000000000..f846cfd6f41 --- /dev/null +++ b/modules/drivers/lidar/lslidar/params/ls32_novatel_extrinsics.yaml @@ -0,0 +1,17 @@ +header: + seq: 0 + stamp: + secs: 1598864145 + nsecs: 0 + frame_id: novatel +transform: + rotation: + x: 0.019 + y: -0.030 + z: -0.009 + w: 0.999 + translation: + x: -0.221985649500604 + y: 0.6272410563007734 + z: 0.0723201225767891 +child_frame_id: lslidar32 diff --git a/modules/drivers/lidar/lslidar/params/ls64w_novatel_extrinsics.yaml b/modules/drivers/lidar/lslidar/params/ls64w_novatel_extrinsics.yaml new file mode 100644 index 00000000000..4fa21a5777e --- /dev/null +++ b/modules/drivers/lidar/lslidar/params/ls64w_novatel_extrinsics.yaml @@ -0,0 +1,17 @@ +header: + seq: 0 + stamp: + secs: 1598864145 + nsecs: 0 + frame_id: novatel +transform: + rotation: + x: -0.03002220039113074 + y: -0.03153447400805393 + z: 0.4900925455685196 + w: 0.870582300079021 + translation: + x: 0.5137241988598309 + y: 0.5743627411243659 + z: 0.3108987234285666 +child_frame_id: lslidarCH64w diff --git a/modules/drivers/lidar/lslidar/parser/BUILD b/modules/drivers/lidar/lslidar/parser/BUILD new file mode 100644 index 00000000000..95ac1386d7f --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/BUILD @@ -0,0 +1,68 @@ +load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library") +load("//tools:cpplint.bzl", "cpplint") +load("//tools/install:install.bzl", "install") + +package(default_visibility = ["//visibility:public"]) + + +install( + name = "install", + library_dest = "drivers/lib/lidar/lslidar/parser", + targets = [ + ":liblslidar_convert_component.so", + ], +) + +cc_binary( + name = "liblslidar_convert_component.so", + linkshared = True, + linkstatic = True, + deps = [":lslidar_convert_component_lib"], +) + +cc_library( + name = "lslidar_convert_component_lib", + srcs = ["lslidar_convert_component.cc"], + hdrs = ["lslidar_convert_component.h"], + copts = ['-DMODULE_NAME=\\"lslidar\\"'], + deps = [ + "//cyber", + "//modules/drivers/lidar/lslidar/parser:convert", + ], + alwayslink = True, +) + +cc_library( + name = "convert", + srcs = [ + "calibration.cc", + "convert.cc", + "lslidar16_parser.cc", + "lslidar32_parser.cc", + "lslidarCXV4_parser.cc", + "lslidarCH16_parser.cc", + "lslidarCH32_parser.cc", + "lslidarCH64_parser.cc", + "lslidarCH64w_parser.cc", + "lslidarCH120_parser.cc", + "lslidarCH128_parser.cc", + "lslidarCH128X1_parser.cc", + "lslidarLS128S2_parser.cc", + "lslidar_parser.cc", + ], + hdrs = [ + "calibration.h", + "convert.h", + "lslidar_parser.h", + ], + copts = ['-DMODULE_NAME=\\"lslidar\\"'], + deps = [ + "//cyber", + "//modules/common_msgs/sensor_msgs:pointcloud_cc_proto", + "//modules/drivers/lidar/lslidar/proto:config_cc_proto", + "//modules/drivers/lidar/lslidar/proto:lslidar_cc_proto", + "@com_github_jbeder_yaml_cpp//:yaml-cpp", + ], +) + +cpplint() diff --git a/modules/drivers/lidar/lslidar/parser/calibration.cc b/modules/drivers/lidar/lslidar/parser/calibration.cc new file mode 100644 index 00000000000..0a34b37cc05 --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/calibration.cc @@ -0,0 +1,229 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +/** + * \file calibration.cc + * \brief + * + * \author Piyush Khandelwal (piyushk@cs.utexas.edu) + * Copyright (C) 2012, Austin Robot Technology, + * The University of Texas at Austin + * + * License: Modified BSD License + * + * $ Id: 02/14/2012 11:36:36 AM piyushk $ + */ + +#include "modules/drivers/lidar/lslidar/parser/calibration.h" + + + +#include "yaml-cpp/yaml.h" + +namespace YAML { +// The >> operator disappeared in yaml-cpp 0.5, so this function is +// added to provide support for code written under the yaml-cpp 0.3 API. +template +void operator>>(const YAML::Node &node, T &i) { + i = node.as(); +} +} // namespace YAML + +namespace apollo { +namespace drivers { +namespace lslidar { + +const char *NUM_LASERS = "num_lasers"; +const char *LASERS = "lasers"; +const char *LASER_ID = "laser_id"; +const char *ROT_CORRECTION = "rot_correction"; +const char *VERT_CORRECTION = "vert_correction"; +const char *DIST_CORRECTION = "dist_correction"; +const char *TWO_PT_CORRECTION_AVAILABLE = "two_pt_correction_available"; +const char *DIST_CORRECTION_X = "dist_correction_x"; +const char *DIST_CORRECTION_Y = "dist_correction_y"; +const char *VERT_OFFSET_CORRECTION = "vert_offset_correction"; +const char *HORIZ_OFFSET_CORRECTION = "horiz_offset_correction"; +const char *MAX_INTENSITY = "max_intensity"; +const char *MIN_INTENSITY = "min_intensity"; +const char *FOCAL_DISTANCE = "focal_distance"; +const char *FOCAL_SLOPE = "focal_slope"; + +void operator>>(const YAML::Node &node, + std::pair &correction) { + node[LASER_ID] >> correction.first; + node[ROT_CORRECTION] >> correction.second.rot_correction; + node[VERT_CORRECTION] >> correction.second.vert_correction; + node[DIST_CORRECTION] >> correction.second.dist_correction; + node[DIST_CORRECTION_X] >> correction.second.dist_correction_x; + node[DIST_CORRECTION_Y] >> correction.second.dist_correction_y; + node[VERT_OFFSET_CORRECTION] >> correction.second.vert_offset_correction; + if (node[HORIZ_OFFSET_CORRECTION]) { + node[HORIZ_OFFSET_CORRECTION] >> correction.second.horiz_offset_correction; + } else { + correction.second.horiz_offset_correction = 0.0; + } + + if (node[MAX_INTENSITY]) { + node[MAX_INTENSITY] >> correction.second.max_intensity; + } else { + correction.second.max_intensity = 255; + } + + if (node[MIN_INTENSITY]) { + node[MIN_INTENSITY] >> correction.second.min_intensity; + } else { + correction.second.min_intensity = 0; + } + + node[FOCAL_DISTANCE] >> correction.second.focal_distance; + node[FOCAL_SLOPE] >> correction.second.focal_slope; + + // Calculate cached values + correction.second.cos_rot_correction = cosf(correction.second.rot_correction); + correction.second.sin_rot_correction = sinf(correction.second.rot_correction); + correction.second.cos_vert_correction = + cosf(correction.second.vert_correction); + correction.second.sin_vert_correction = + sinf(correction.second.vert_correction); + correction.second.focal_offset = + 256.0f * static_cast(std::pow( + 1 - correction.second.focal_distance / 13100.0f, 2)); + correction.second.laser_ring = 0; // clear initially (set later) +} + +void operator>>(const YAML::Node &node, Calibration &calibration) { + int num_lasers = 0; + node[NUM_LASERS] >> num_lasers; + const YAML::Node &lasers = node[LASERS]; + calibration.laser_corrections_.clear(); + calibration.num_lasers_ = num_lasers; + + for (int i = 0; i < num_lasers; i++) { + std::pair correction; + lasers[i] >> correction; + calibration.laser_corrections_.insert(correction); + } + + // For each laser ring, find the next-smallest vertical angle. + // + // This implementation is simple, but not efficient. That is OK, + // since it only runs while starting up. + double next_angle = -std::numeric_limits::infinity(); + + for (int ring = 0; ring < num_lasers; ++ring) { + // find minimum remaining vertical offset correction + double min_seen = std::numeric_limits::infinity(); + int next_index = num_lasers; + + for (int j = 0; j < num_lasers; ++j) { + double angle = calibration.laser_corrections_[j].vert_correction; + + if (next_angle < angle && angle < min_seen) { + min_seen = angle; + next_index = j; + } + } + + if (next_index < num_lasers) { // anything found in this ring? + // store this ring number with its corresponding laser number + calibration.laser_corrections_[next_index].laser_ring = ring; + next_angle = min_seen; + } + } +} + +YAML::Emitter &operator<<(YAML::Emitter &out, + const std::pair &correction) { + out << YAML::BeginMap; + out << YAML::Key << LASER_ID << YAML::Value << correction.first; + out << YAML::Key << ROT_CORRECTION << YAML::Value + << correction.second.rot_correction; + out << YAML::Key << VERT_CORRECTION << YAML::Value + << correction.second.vert_correction; + out << YAML::Key << DIST_CORRECTION << YAML::Value + << correction.second.dist_correction; + out << YAML::Key << DIST_CORRECTION_X << YAML::Value + << correction.second.dist_correction_x; + out << YAML::Key << DIST_CORRECTION_Y << YAML::Value + << correction.second.dist_correction_y; + out << YAML::Key << VERT_OFFSET_CORRECTION << YAML::Value + << correction.second.vert_offset_correction; + out << YAML::Key << HORIZ_OFFSET_CORRECTION << YAML::Value + << correction.second.horiz_offset_correction; + out << YAML::Key << MAX_INTENSITY << YAML::Value + << correction.second.max_intensity; + out << YAML::Key << MIN_INTENSITY << YAML::Value + << correction.second.min_intensity; + out << YAML::Key << FOCAL_DISTANCE << YAML::Value + << correction.second.focal_distance; + out << YAML::Key << FOCAL_SLOPE << YAML::Value + << correction.second.focal_slope; + out << YAML::EndMap; + return out; +} + +YAML::Emitter &operator<<(YAML::Emitter &out, const Calibration &calibration) { + out << YAML::BeginMap; + out << YAML::Key << NUM_LASERS << YAML::Value + << calibration.laser_corrections_.size(); + out << YAML::Key << LASERS << YAML::Value << YAML::BeginSeq; + + for (std::map::const_iterator it = + calibration.laser_corrections_.begin(); + it != calibration.laser_corrections_.end(); ++it) { + out << *it; + } + + out << YAML::EndSeq; + out << YAML::EndMap; + return out; +} + +void Calibration::read(const std::string &calibration_file) { + std::ifstream fin(calibration_file.c_str()); + + if (!fin.is_open()) { + initialized_ = false; + return; + } + + initialized_ = true; + + try { + YAML::Node doc; + fin.close(); + doc = YAML::LoadFile(calibration_file); + doc >> *this; + } catch (YAML::Exception &e) { + std::cerr << "YAML Exception: " << e.what() << std::endl; + initialized_ = false; + } + + fin.close(); +} + +void Calibration::write(const std::string &calibration_file) { + std::ofstream fout(calibration_file.c_str()); + YAML::Emitter out; + out << *this; + fout << out.c_str(); + fout.close(); +} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/calibration.h b/modules/drivers/lidar/lslidar/parser/calibration.h new file mode 100644 index 00000000000..d789d9874d4 --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/calibration.h @@ -0,0 +1,87 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +/** + * \file calibration.h + * + * \author Piyush Khandelwal (piyushk@cs.utexas.edu) + * Copyright (C) 2012, Austin Robot Technology, University of Texas at Austin + * + * License: Modified BSD License + * + * $ Id: 02/14/2012 11:25:34 AM piyushk $ + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace apollo { +namespace drivers { +namespace lslidar { + +/** \brief correction values for a single laser**/ + +struct LaserCorrection { + /** parameters in db.xml */ + float rot_correction; + float vert_correction; + float dist_correction; + float dist_correction_x; + float dist_correction_y; + float vert_offset_correction; + float horiz_offset_correction; + int max_intensity; + int min_intensity; + float focal_distance; + float focal_slope; + float focal_offset; + + /** cached values calculated when the calibration file is read */ + float cos_rot_correction; ///< cached cosine of rot_correction + float sin_rot_correction; ///< cached sine of rot_correction + float cos_vert_correction; ///< cached cosine of vert_correction + float sin_vert_correction; ///< cached sine of vert_correction + + int laser_ring; ///< ring number for this laser +}; + +/** \brief Calibration class storing entire configuration for the Lslidar */ +class Calibration { + public: + std::map laser_corrections_; + int num_lasers_; + bool initialized_; + + public: + Calibration() : initialized_(false) {} + explicit Calibration(const std::string& calibration_file) { + read(calibration_file); + } + + void read(const std::string& calibration_file); + void write(const std::string& calibration_file); +}; + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/convert.cc b/modules/drivers/lidar/lslidar/parser/convert.cc new file mode 100644 index 00000000000..a34a74062ff --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/convert.cc @@ -0,0 +1,59 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/parser/convert.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +using apollo::drivers::PointCloud; +using apollo::drivers::lslidar::LslidarScan; + +void Convert::init(const Config& lslidar_config) { + config_ = lslidar_config; + // we use Beijing time by default + + AERROR << "Convert::init"; + parser_.reset(LslidarParserFactory::CreateParser(config_)); + if (parser_.get() == nullptr) { + AFATAL << "Create parser failed."; + return; + } + parser_->setup(); +} + +/** @brief Callback for raw scan messages. */ +void Convert::ConvertPacketsToPointcloud( + const std::shared_ptr& scan_msg, + std::shared_ptr point_cloud) { + AINFO_EVERY(100) << "Converting scan msg seq " + << scan_msg->header().sequence_num(); + + parser_->GeneratePointcloud(scan_msg, point_cloud); + + if (point_cloud == nullptr || point_cloud->point().empty()) { + AERROR << "point cloud has no point"; + return; + } + + parser_->Order(point_cloud); + point_cloud->set_is_dense(false); +} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/convert.h b/modules/drivers/lidar/lslidar/parser/convert.h new file mode 100644 index 00000000000..17a108c9d6d --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/convert.h @@ -0,0 +1,61 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include +#include + +// #include "modules/drivers/proto/pointcloud.pb.h" +#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h" +#include "modules/drivers/lidar/lslidar/proto/config.pb.h" +#include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" + +#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +using apollo::drivers::PointCloud; +using apollo::drivers::lslidar::LslidarScan; + +// convert lslidar data to pointcloud and republish +class Convert { + public: + Convert() = default; + virtual ~Convert() = default; + + // init lslidar config struct from private_nh + void init(const Config& lslidar_config); + + // convert lslidar data to pointcloud and public + void ConvertPacketsToPointcloud( + const std::shared_ptr& scan_msg, + std::shared_ptr point_cloud_out); + + private: + // RawData class for converting data to point cloud + std::unique_ptr parser_; + std::string channel_pointcloud_; + + /// configuration parameters, get config struct from lslidar_parser.h + Config config_; +}; + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidar16_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidar16_parser.cc new file mode 100644 index 00000000000..73411d89393 --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidar16_parser.cc @@ -0,0 +1,385 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +float R1_ = 0.04376; +float R2_ = 0.010875; + +Lslidar16Parser::Lslidar16Parser(const Config &config) + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) { + need_two_pt_correction_ = false; + config_vert_angle = false; + previous_packet_time = 0; + + if (2 == config_.degree_mode()) { + for (int i = 0; i < LSC16_SCANS_PER_FIRING; i++) { + cos_scan_altitude_caliration[i] = std::cos(scan_altitude_original_2[i]); + sin_scan_altitude_caliration[i] = std::sin(scan_altitude_original_2[i]); + scan_altitude[i] = scan_altitude_original_2[i]; + } + } else if (1 == config_.degree_mode()) { + for (int i = 0; i < LSC16_SCANS_PER_FIRING; i++) { + cos_scan_altitude_caliration[i] = std::cos(scan_altitude_original_1[i]); + sin_scan_altitude_caliration[i] = std::sin(scan_altitude_original_1[i]); + scan_altitude[i] = scan_altitude_original_1[i]; + } + } + + for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { + float rotation = + ROTATION_RESOLUTION * static_cast(rot_index * M_PI) / 180.0f; + cos_azimuth_table[rot_index] = cosf(rotation); + sin_azimuth_table[rot_index] = sinf(rotation); + } +} + +void Lslidar16Parser::GeneratePointcloud( + const std::shared_ptr &scan_msg, + const std::shared_ptr &out_msg) { + // allocate a point cloud with same time and frame ID as raw data + out_msg->mutable_header()->set_timestamp_sec(scan_msg->basetime() / + 1000000000.0); + out_msg->mutable_header()->set_module_name(scan_msg->header().module_name()); + out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id()); + out_msg->set_height(1); + out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0); + out_msg->mutable_header()->set_sequence_num( + scan_msg->header().sequence_num()); + gps_base_usec_ = scan_msg->basetime(); + + const unsigned char *difop_ptr = + (const unsigned char *)scan_msg->difop_pkts(0).data().c_str(); + uint8_t version_data = static_cast(difop_ptr[1202]); + + if (config_.config_vert()) { + if (2 == version_data) { + for (int i = 0; i < 16; i++) { + uint16_t vert_angle = static_cast( + difop_ptr[234 + 2 * i] * 256 + difop_ptr[234 + 2 * i + 1]); + + if (vert_angle > 32767) { + vert_angle = vert_angle - 65535; + } + + scan_altitude[i] = + (static_cast(vert_angle) / 100.f) * DEG_TO_RAD; + if (2 == config_.degree_mode()) { + if (scan_altitude[i] != 0) { + if (fabs(scan_altitude_original_2[i] - scan_altitude[i]) * + RAD_TO_DEG > + 1.5) { + scan_altitude[i] = scan_altitude_original_2[i]; + } + } else { + scan_altitude[i] = scan_altitude_original_2[i]; + } + } else if (1 == config_.degree_mode()) { + if (scan_altitude[i] != 0) { + if (fabs(scan_altitude_original_1[i] - scan_altitude[i]) * + RAD_TO_DEG > + 1.5) { + scan_altitude[i] = scan_altitude_original_1[i]; + } + } else { + scan_altitude[i] = scan_altitude_original_1[i]; + } + } + config_vert_angle = true; + } + } else { + for (int i = 0; i < 16; i++) { + uint16_t vert_angle = static_cast( + difop_ptr[245 + 2 * i] * 256 + difop_ptr[245 + 2 * i + 1]); + + if (vert_angle > 32767) { + vert_angle = vert_angle - 65535; + } + + scan_altitude[i] = + (static_cast(vert_angle) / 100.f) * DEG_TO_RAD; + + if (2 == config_.degree_mode()) { + if (scan_altitude[i] != 0) { + if (fabs(scan_altitude_original_2[i] - scan_altitude[i]) * + RAD_TO_DEG > + 1.5) { + scan_altitude[i] = scan_altitude_original_2[i]; + } + } else { + scan_altitude[i] = scan_altitude_original_2[i]; + } + } else if (1 == config_.degree_mode()) { + if (scan_altitude[i] != 0) { + if (fabs(scan_altitude_original_1[i] - scan_altitude[i]) * + RAD_TO_DEG > + 1.5) { + scan_altitude[i] = scan_altitude_original_1[i]; + } + } else { + scan_altitude[i] = scan_altitude_original_1[i]; + } + } + config_vert_angle = true; + } + } + } + + size_t packets_size = scan_msg->firing_pkts_size(); + packet_number_ = packets_size; + block_num = 0; + + for (size_t i = 0; i < packets_size; ++i) { + Unpack(scan_msg->firing_pkts(static_cast(i)), out_msg, i); + last_time_stamp_ = out_msg->measurement_time(); + ADEBUG << "***************stamp: " << std::fixed << last_time_stamp_; + } + + if (out_msg->point().empty()) { + // we discard this pointcloud if empty + AERROR << "All points is NAN!Please check lslidar:" << config_.model(); + } + // set default width + out_msg->set_width(out_msg->point_size()); +} + +/** @brief convert raw packet to point cloud + * @param pkt raw packet to Unpack + * @param pc shared pointer to point cloud (points are appended) + */ +void Lslidar16Parser::Unpack(const LslidarPacket &pkt, + std::shared_ptr pc, + int packet_number) { + float x, y, z; + float azimuth = 0.0f; + uint32_t intensity = 0; + float azimuth_diff = 0.0f; + float azimuth_corrected_f = 0.0f; + int azimuth_corrected = 0; + uint64_t point_time; + uint64_t packet_end_time; + + if (config_vert_angle) { + for (int i = 0; i < LSC16_SCANS_PER_FIRING; i++) { + cos_scan_altitude_caliration[i] = std::cos(scan_altitude[i]); + sin_scan_altitude_caliration[i] = std::sin(scan_altitude[i]); + } + config_vert_angle = false; + } + const raw_packet_t *raw = (const raw_packet_t *)pkt.data().c_str(); + + if (!config_.time_synchronization()) { + packet_end_time = pkt.stamp(); + } else { + packet_end_time = pkt.stamp(); + } + current_packet_time = packet_end_time; + int point_count = -1; + for (int block = 0; block < BLOCKS_PER_PACKET; block++, block_num++) { + if (UPPER_BANK != raw->blocks[block].header) break; + + azimuth = static_cast(256 * raw->blocks[block].rotation_2 + + raw->blocks[block].rotation_1); + + if (2 == config_.return_mode()) { + if (block < (BLOCKS_PER_PACKET - 2)) { + int azi1, azi2; + azi1 = 256 * raw->blocks[block + 2].rotation_2 + + raw->blocks[block + 2].rotation_1; + azi2 = + 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; + azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); + } else { + int azi1, azi2; + azi1 = + 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; + azi2 = 256 * raw->blocks[block - 2].rotation_2 + + raw->blocks[block - 2].rotation_1; + azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); + } + } else { + if (block < (BLOCKS_PER_PACKET - 1)) { + int azi1, azi2; + azi1 = 256 * raw->blocks[block + 1].rotation_2 + + raw->blocks[block + 1].rotation_1; + azi2 = + 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; + azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); + } else { + int azi1, azi2; + azi1 = + 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; + azi2 = 256 * raw->blocks[block - 1].rotation_2 + + raw->blocks[block - 1].rotation_1; + azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); + } + } + + float cos_azimuth; + float sin_azimuth; + + for (int firing = 0, k = 0; firing < LSC16_FIRINGS_PER_BLOCK; ++firing) { + for (int dsr = 0; dsr < LSC16_SCANS_PER_FIRING; + ++dsr, k += RAW_SCAN_SIZE) { + point_count++; + LaserCorrection &corrections = calibration_.laser_corrections_[dsr]; + + /** correct for the laser rotation as a function of timing during the + * firings **/ + azimuth_corrected_f = + azimuth + azimuth_diff / (LSC16_SCANS_PER_FIRING * 2) * + (LSC16_SCANS_PER_FIRING * firing + dsr); + + azimuth_corrected = + static_cast(round(fmod(azimuth_corrected_f, 36000.0))); + + cos_azimuth = cos_azimuth_table[azimuth_corrected]; + sin_azimuth = sin_azimuth_table[azimuth_corrected]; + + // distance + union two_bytes tmp; + tmp.bytes[0] = raw->blocks[block].data[k]; + tmp.bytes[1] = raw->blocks[block].data[k + 1]; + int distance = tmp.uint; + + // read intensity + intensity = raw->blocks[block].data[k + 2]; + + float distance2 = + (distance * DISTANCE_RESOLUTION) * config_.distance_unit(); + + if (config_.calibration()) + distance2 = distance2 + corrections.dist_correction; + + // The offset calibration + float arg_horiz = + static_cast(azimuth_corrected_f * ROTATION_RESOLUTION); + arg_horiz = arg_horiz > 360 ? (arg_horiz - 360) : arg_horiz; + float arg_horiz_orginal = (14.67 - arg_horiz) * M_PI / 180; + + if (2 == config_.return_mode()) { + // modify + if (previous_packet_time > 1e-6) { + point_time = current_packet_time - + (SCANS_PER_PACKET - point_count - 1) * + (current_packet_time - previous_packet_time) / + (SCANS_PER_PACKET); + } else { // fist packet + point_time = current_packet_time; + } + + } else { + // modify + if (previous_packet_time > 1e-6) { + point_time = current_packet_time - + (SCANS_PER_PACKET - point_count - 1) * + (current_packet_time - previous_packet_time) / + (SCANS_PER_PACKET); + } else { // fist packet + point_time = current_packet_time; + } + } + + if ((azimuth_corrected < config_.scan_start_angle()) || + (azimuth_corrected > config_.scan_end_angle())) + continue; + if (packet_number == 0) { + if (azimuth_corrected > 30000) { + continue; + } + } + + if (packet_number == packet_number_ - 1) { + if (azimuth_corrected < 10000) { + continue; + } + } + + if (distance2 > config_.max_range() || + distance2 < config_.min_range()) { + PointXYZIT *point = pc->add_point(); + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + PointXYZIT *point = pc->add_point(); + point->set_timestamp(point_time); + + point->set_intensity(intensity); + + if (config_.calibration()) { + ComputeCoords(distance2, &corrections, + static_cast(azimuth_corrected), point); + + } else { + x = distance2 * cos_scan_altitude_caliration[dsr] * cos_azimuth + + R1_ * cos(arg_horiz_orginal); + y = -distance2 * cos_scan_altitude_caliration[dsr] * sin_azimuth + + R1_ * sin(arg_horiz_orginal); + z = distance2 * sin_scan_altitude_caliration[dsr] + 0.426 / 100.f; + + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && + -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); + } + } + } + } + } + } + previous_packet_time = current_packet_time; +} + +void Lslidar16Parser::Order(std::shared_ptr cloud) { + int width = 16; + cloud->set_width(width); + int height = cloud->point_size() / cloud->width(); + cloud->set_height(height); + + std::shared_ptr cloud_origin = std::make_shared(); + cloud_origin->CopyFrom(*cloud); + + for (int i = 0; i < width; ++i) { + int col = lslidar::ORDER_16[i]; + + for (int j = 0; j < height; ++j) { + // make sure offset is initialized, should be init at setup() just once + int target_index = j * width + i; + int origin_index = j * width + col; + cloud->mutable_point(target_index) + ->CopyFrom(cloud_origin->point(origin_index)); + } + } +} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidar32_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidar32_parser.cc new file mode 100644 index 00000000000..badbfe22261 --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidar32_parser.cc @@ -0,0 +1,555 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +Lslidar32Parser::Lslidar32Parser(const Config& config) + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) { + adjust_angle = 0; + adjust_angle_two = 0; + adjust_angle_three = 0; + adjust_angle_four = 0; + read_difop_ = true; + config_vert_angle = false; + lslidar_type = 2; +} + +void Lslidar32Parser::GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg) { + // allocate a point cloud with same time and frame ID as raw data + out_msg->mutable_header()->set_timestamp_sec(scan_msg->basetime() / + 1000000000.0); + out_msg->mutable_header()->set_module_name(scan_msg->header().module_name()); + out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id()); + out_msg->set_height(1); + out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0); + out_msg->mutable_header()->set_sequence_num( + scan_msg->header().sequence_num()); + gps_base_usec_ = scan_msg->basetime(); + + double scan_altitude_original_degree33[32]; + double scan_altitude_original_degree1[32]; + int startpos = 0; + + const unsigned char* difop_ptr = + (const unsigned char*)scan_msg->difop_pkts(0).data().c_str(); + if (difop_ptr[0] == 0xa5 && difop_ptr[1] == 0xff && difop_ptr[2] == 0x00 && + difop_ptr[3] == 0x5a) { + if ((difop_ptr[1202] >= 0x02 && difop_ptr[1203] >= 0x80) || + difop_ptr[1202] == 0x03) { + lslidar_type = 3; + startpos = 245; + // Horizontal correction Angle + adjust_angle = + (difop_ptr[186] * 256 + difop_ptr[187]); // Angle correction A1 + if (adjust_angle > 32767) { + adjust_angle = adjust_angle - 65535; + } + adjust_angle_two = + (difop_ptr[190] * 256 + difop_ptr[191]); // Angle correction A2 + if (adjust_angle_two > 32767) { + adjust_angle_two = adjust_angle_two - 65535; + } + adjust_angle_three = + (difop_ptr[188] * 256 + difop_ptr[189]); // Angle correction A3 + if (adjust_angle_three > 32767) { + adjust_angle_three = adjust_angle_three - 65535; + } + adjust_angle_four = + (difop_ptr[192] * 256 + difop_ptr[193]); // Angle correction A4 + if (adjust_angle_four > 32767) { + adjust_angle_four = adjust_angle_four - 65535; + } + memcpy(scan_altitude_original_degree1, scan_altitude_original_A3, 32 * 8); + memcpy(scan_altitude_original_degree33, scan_altitude_original_C3, + 32 * 8); + + if (difop_ptr[185] == 0 || difop_ptr[185] == 1) { + int return_mode = difop_ptr[185] + 1; + config_.set_return_mode(return_mode); + + if (difop_ptr[1195] == 0x21) + config_.set_degree_mode(2); + else + config_.set_degree_mode(1); + + config_.set_distance_unit(0.4f); + + for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) { + // 均匀1度校准两列 + if (1 == config_.degree_mode()) { + cos_scan_altitude_caliration[i] = std::cos( + static_cast(scan_altitude_original_A3[i] * M_PI) / + 180.0f); + sin_scan_altitude_caliration[i] = std::sin( + static_cast(scan_altitude_original_A3[i] * M_PI) / + 180.0f); + scan_altitude_A[i] = scan_altitude_original_A3[i]; + } + // 0.33度校准四列 + if (2 == config_.degree_mode()) { + cos_scan_altitude_caliration[i] = std::cos( + static_cast(scan_altitude_original_C3[i] * M_PI) / + 180.0f); + sin_scan_altitude_caliration[i] = std::sin( + static_cast(scan_altitude_original_C3[i] * M_PI) / + 180.0f); + scan_altitude_C[i] = scan_altitude_original_C3[i]; + } + } + } + } else { + lslidar_type = 2; + startpos = 882; + // Horizontal correction Angle + adjust_angle = + (difop_ptr[34] * 256 + difop_ptr[35]); /*Angle correction A1*/ + if (adjust_angle > 32767) { + adjust_angle = adjust_angle - 65535; + } + adjust_angle_two = + (difop_ptr[42] * 256 + difop_ptr[43]); /*Angle correction A2*/ + if (adjust_angle_two > 32767) { + adjust_angle_two = adjust_angle_two - 65535; + } + adjust_angle_three = + (difop_ptr[66] * 256 + difop_ptr[67]); /*Angle correction A3*/ + if (adjust_angle_three > 32767) { + adjust_angle_three = adjust_angle_three - 65535; + } + adjust_angle_four = + (difop_ptr[68] * 256 + difop_ptr[69]); /*Angle correction A4*/ + if (adjust_angle_four > 32767) { + adjust_angle_four = adjust_angle_four - 65535; + } + AWARN << "Load config failed, config file"; + // Vertical Angle Calibration for device package + for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) { + // 均匀1度校准两列 + if (1 == config_.degree_mode()) { + cos_scan_altitude_caliration[i] = std::cos( + static_cast(scan_altitude_original_A[i] * M_PI) / 180.0f); + sin_scan_altitude_caliration[i] = std::sin( + static_cast(scan_altitude_original_A[i] * M_PI) / 180.0f); + scan_altitude_A[i] = scan_altitude_original_A[i]; + } + // 0.33度校准四列 + if (2 == config_.degree_mode()) { + cos_scan_altitude_caliration[i] = std::cos( + static_cast(scan_altitude_original_C[i] * M_PI) / 180.0f); + sin_scan_altitude_caliration[i] = std::sin( + static_cast(scan_altitude_original_C[i] * M_PI) / 180.0f); + scan_altitude_C[i] = scan_altitude_original_C[i]; + } + } + memcpy(scan_altitude_original_degree1, scan_altitude_original_A, 32 * 8); + memcpy(scan_altitude_original_degree33, scan_altitude_original_C, 32 * 8); + } + } + + // Vertical Angle parse + if (config_.config_vert()) { + for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) { + uint8_t data1 = difop_ptr[startpos + 2 * i]; + uint8_t data2 = difop_ptr[startpos + 2 * i + 1]; + int vert_angle = data1 * 256 + data2; + if (vert_angle > 32767) { + vert_angle = vert_angle - 65535; + } + // 均匀1度校准两列 + if (1 == config_.degree_mode()) { + scan_altitude_A[i] = + static_cast(vert_angle * ROTATION_RESOLUTION); + if (fabs(scan_altitude_original_degree1[i] - scan_altitude_A[i]) > + 1.5) { + scan_altitude_A[i] = scan_altitude_original_degree1[i]; + } + config_vert_angle = true; + } + // 0.33度校准四列 + if (2 == config_.degree_mode()) { + scan_altitude_C[i] = + static_cast(vert_angle * ROTATION_RESOLUTION); + if (fabs(scan_altitude_original_degree33[i] - scan_altitude_C[i]) > + 1.5) { + scan_altitude_C[i] = scan_altitude_original_degree33[i]; + } + config_vert_angle = true; + } + } + } + + size_t packets_size = scan_msg->firing_pkts_size(); + block_num = 0; + packet_number_ = packets_size; + + AINFO << "packets_size :" << packets_size; + + for (size_t i = 0; i < packets_size; ++i) { + Unpack(scan_msg->firing_pkts(static_cast(i)), out_msg, i); + last_time_stamp_ = out_msg->measurement_time(); + ADEBUG << "stamp: " << std::fixed << last_time_stamp_; + } + + if (out_msg->point().empty()) { + // we discard this pointcloud if empty + AERROR << "All points is NAN!Please check lslidar:" << config_.model(); + } + + // set default width + out_msg->set_width(out_msg->point_size()); +} + +/** @brief convert raw packet to point cloud + * @param pkt raw packet to Unpack + * @param pc shared pointer to point cloud (points are appended) + */ +void Lslidar32Parser::Unpack(const LslidarPacket& pkt, + std::shared_ptr pc, + int packet_number) { + float x, y, z; + float azimuth = 0.0f; + uint32_t intensity = 0; + float azimuth_diff = 0.0f; + float azimuth_corrected_f = 0.0f; + float azimuth_corrected_offset_f = 0.0f; + uint64_t point_time; + uint64_t packet_end_time; + + if (config_vert_angle) { + for (int i = 0; i < LSC32_SCANS_PER_FIRING; i++) { + // 均匀1度校准两列 + if (1 == config_.degree_mode()) { + cos_scan_altitude_caliration[i] = + std::cos(static_cast(scan_altitude_A[i] * M_PI) / 180.0f); + sin_scan_altitude_caliration[i] = + std::sin(static_cast(scan_altitude_A[i] * M_PI) / 180.0f); + } + + // 0.33度校准四列 + if (2 == config_.degree_mode()) { + cos_scan_altitude_caliration[i] = + std::cos(static_cast(scan_altitude_C[i] * M_PI) / 180.0f); + sin_scan_altitude_caliration[i] = + std::sin(static_cast(scan_altitude_C[i] * M_PI) / 180.0f); + } + } + config_vert_angle = false; + } + + const raw_packet_t* raw = (const raw_packet_t*)pkt.data().c_str(); + if (!config_.time_synchronization()) { + packet_end_time = pkt.stamp(); + } else { + packet_end_time = pkt.stamp(); + } + current_packet_time = packet_end_time; + + int point_count = -1; + for (int block = 0; block < BLOCKS_PER_PACKET; block++, block_num++) { + if (UPPER_BANK != raw->blocks[block].header) break; + + azimuth = static_cast(256 * raw->blocks[block].rotation_2 + + raw->blocks[block].rotation_1); + + if (2 == config_.return_mode()) { + if (block < (BLOCKS_PER_PACKET - 2)) { + int azi1, azi2; + azi1 = 256 * raw->blocks[block + 2].rotation_2 + + raw->blocks[block + 2].rotation_1; + azi2 = + 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; + azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); + } else { + int azi1, azi2; + azi1 = + 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; + azi2 = 256 * raw->blocks[block - 2].rotation_2 + + raw->blocks[block - 2].rotation_1; + azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); + } + azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff; + azimuth_diff = azimuth_diff > 36000 ? azimuth_diff - 36000 : azimuth_diff; + } else { + if (block < (BLOCKS_PER_PACKET - 1)) { + int azi1, azi2; + azi1 = 256 * raw->blocks[block + 1].rotation_2 + + raw->blocks[block + 1].rotation_1; + azi2 = + 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; + azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); + } else { + int azi1, azi2; + azi1 = + 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1; + azi2 = 256 * raw->blocks[block - 1].rotation_2 + + raw->blocks[block - 1].rotation_1; + azimuth_diff = static_cast((36000 + azi1 - azi2) % 36000); + } + azimuth_diff = azimuth_diff < 0 ? azimuth_diff + 36000 : azimuth_diff; + azimuth_diff = azimuth_diff > 36000 ? azimuth_diff - 36000 : azimuth_diff; + } + + for (int firing = 0, k = 0; firing < LSC32_FIRINGS_PER_BLOCK; ++firing) { + for (int dsr = 0; dsr < LSC32_SCANS_PER_FIRING; + ++dsr, k += RAW_SCAN_SIZE) { + point_count++; + /** correct for the laser rotation as a function of timing during the + * firings **/ + LaserCorrection& corrections = calibration_.laser_corrections_[dsr]; + + azimuth_corrected_f = + azimuth + azimuth_diff / LSC32_SCANS_PER_FIRING * dsr; + azimuth_corrected_f = azimuth_corrected_f < 0 + ? azimuth_corrected_f + 36000 + : azimuth_corrected_f; + azimuth_corrected_f = azimuth_corrected_f > 36000 + ? azimuth_corrected_f - 36000 + : azimuth_corrected_f; + + // distance + union two_bytes tmp; + tmp.bytes[0] = raw->blocks[block].data[k]; + tmp.bytes[1] = raw->blocks[block].data[k + 1]; + int distance = tmp.uint; + + // read intensity + intensity = raw->blocks[block].data[k + 2]; + + float distance2 = + (distance * DISTANCE_RESOLUTION) * config_.distance_unit(); + + if (config_.calibration()) + distance2 = distance2 + corrections.dist_correction; + + if (2 == config_.return_mode()) { + if (previous_packet_time > 1e-6) { + point_time = current_packet_time - + (SCANS_PER_PACKET - point_count - 1) * + (current_packet_time - previous_packet_time) / + (SCANS_PER_PACKET); + } else { // fist packet + point_time = current_packet_time; + } + + } else { + if (previous_packet_time > 1e-6) { + point_time = current_packet_time - + (SCANS_PER_PACKET - point_count - 1) * + (current_packet_time - previous_packet_time) / + (SCANS_PER_PACKET); + } else { // fist packet + point_time = current_packet_time; + } + } + + if (distance2 > config_.max_range() || + distance2 < config_.min_range()) { + PointXYZIT* point = pc->add_point(); + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + // 均匀1度校准两列 + if (1 == config_.degree_mode()) { + double adjust_diff = adjust_angle_two - adjust_angle; + if (adjust_diff > 300 && adjust_diff < 460) { + // v2.7 calibrtation + if (lslidar_type == 3) { + if (1 >= dsr % 4) { + azimuth_corrected_f += adjust_angle_two; + } else { + azimuth_corrected_f += adjust_angle; + } + } else { + if (0 == dsr % 2) { + azimuth_corrected_f += adjust_angle_two; + } else { + azimuth_corrected_f += adjust_angle; + } + } + } else { + // v2.6 calibrtation + if (0 == dsr % 2) { + azimuth_corrected_f += adjust_angle; + } else { + azimuth_corrected_f -= adjust_angle; + } + } + } + + // 0.33度校准四列 + if (2 == config_.degree_mode()) { + double adjust_diff_one = adjust_angle_two - adjust_angle; + double adjust_diff_two = adjust_angle_four - adjust_angle_three; + if (lslidar_type == 3) { + // v3.0 calibrtation + if (0 == dsr || 1 == dsr || 4 == dsr || 8 == dsr || 9 == dsr || + 12 == dsr || 16 == dsr || 17 == dsr || 21 == dsr || + 24 == dsr || 25 == dsr || 29 == dsr) + azimuth_corrected_f += adjust_angle_four; // A4 + + if (2 == dsr || 3 == dsr || 6 == dsr || 10 == dsr || 11 == dsr || + 14 == dsr || 18 == dsr || 19 == dsr || 23 == dsr || + 26 == dsr || 27 == dsr || 31 == dsr) + azimuth_corrected_f += adjust_angle_three; // A3 + + if (5 == dsr || 13 == dsr || 20 == dsr || 28 == dsr) + azimuth_corrected_f += adjust_angle_two; // A2 + + if (7 == dsr || 15 == dsr || 22 == dsr || 30 == dsr) + azimuth_corrected_f += adjust_angle; // A1 + } else if (adjust_diff_one > 500 && adjust_diff_one < 660 && + adjust_diff_two > 150 && adjust_diff_two < 350) { + // v2.7 calibrtation + if (10 == dsr || 14 == dsr || 18 == dsr || 22 == dsr) + azimuth_corrected_f += adjust_angle_four; // A4 + + if (11 == dsr || 15 == dsr || 19 == dsr || 23 == dsr) + azimuth_corrected_f += adjust_angle_three; // A3 + + if (0 == dsr || 2 == dsr || 4 == dsr || 6 == dsr || 8 == dsr || + 12 == dsr || 16 == dsr || 20 == dsr || 24 == dsr || + 26 == dsr || 28 == dsr || 30 == dsr) + azimuth_corrected_f += adjust_angle_two; // A2 + + if (1 == dsr || 3 == dsr || 5 == dsr || 7 == dsr || 9 == dsr || + 13 == dsr || 17 == dsr || 21 == dsr || 25 == dsr || + 27 == dsr || 29 == dsr || 31 == dsr) + azimuth_corrected_f += adjust_angle; // A1 + } else { + // v2.6 calibrtation + if (10 == dsr || 14 == dsr || 18 == dsr || 22 == dsr) + azimuth_corrected_f += adjust_angle; + + if (11 == dsr || 15 == dsr || 19 == dsr || 23 == dsr) + azimuth_corrected_f -= adjust_angle; + + if (0 == dsr || 2 == dsr || 4 == dsr || 6 == dsr || 8 == dsr || + 12 == dsr || 16 == dsr || 20 == dsr || 24 == dsr || + 26 == dsr || 28 == dsr || 30 == dsr) + azimuth_corrected_f += adjust_angle_two; + + if (1 == dsr || 3 == dsr || 5 == dsr || 7 == dsr || 9 == dsr || + 13 == dsr || 17 == dsr || 21 == dsr || 25 == dsr || + 27 == dsr || 29 == dsr || 31 == dsr) + azimuth_corrected_f -= adjust_angle_two; + } + } + + azimuth_corrected_f = azimuth_corrected_f < 0 + ? azimuth_corrected_f + 36000 + : azimuth_corrected_f; + azimuth_corrected_f = azimuth_corrected_f > 36000 + ? azimuth_corrected_f - 36000 + : azimuth_corrected_f; + + if ((azimuth_corrected_f < config_.scan_start_angle()) || + (azimuth_corrected_f > config_.scan_end_angle())) + continue; + if (packet_number == 0) { + if (azimuth_corrected_f > 30000) { + continue; + } + } + + if (packet_number == packet_number_ - 1) { + if (azimuth_corrected_f < 10000) { + continue; + } + } + + // 以结构为中心 + float rotation_azimuth = + ROTATION_RESOLUTION * + static_cast(azimuth_corrected_f * M_PI) / 180.0f; + azimuth_corrected_offset_f = + azimuth_corrected_f * ROTATION_RESOLUTION - LSC32_AZIMUTH_TOFFSET; + float rotation_azimuth_offset = + static_cast(azimuth_corrected_offset_f * M_PI) / 180.0f; + + PointXYZIT* point = pc->add_point(); + point->set_timestamp(point_time); + point->set_intensity(intensity); + point->set_intensity(intensity); + + if (config_.calibration()) { + ComputeCoords(distance2, &corrections, + static_cast(azimuth_corrected_f), point); + } else { + x = distance2 * cos_scan_altitude_caliration[dsr] * + cosf(rotation_azimuth) + + (LSC32_DISTANCE_TOFFSET * cosf(rotation_azimuth_offset)) * + DISTANCE_RESOLUTION; + y = -(distance2 * cos_scan_altitude_caliration[dsr] * + sinf(rotation_azimuth) + + (LSC32_DISTANCE_TOFFSET * sinf(rotation_azimuth_offset)) * + DISTANCE_RESOLUTION); + z = distance2 * sin_scan_altitude_caliration[dsr]; + + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && + -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); + } + } + } + } + } + } + previous_packet_time = current_packet_time; +} + +void Lslidar32Parser::Order(std::shared_ptr cloud) { + int width = 32; + cloud->set_width(width); + int height = cloud->point_size() / cloud->width(); + cloud->set_height(height); + + std::shared_ptr cloud_origin = std::make_shared(); + cloud_origin->CopyFrom(*cloud); + + for (int i = 0; i < width; ++i) { + int col = lslidar::ORDER_32[i]; + + for (int j = 0; j < height; ++j) { + // make sure offset is initialized, should be init at setup() just once + int target_index = j * width + i; + int origin_index = j * width + col; + cloud->mutable_point(target_index) + ->CopyFrom(cloud_origin->point(origin_index)); + } + } +} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCH120_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCH120_parser.cc new file mode 100644 index 00000000000..10abf3507a1 --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidarCH120_parser.cc @@ -0,0 +1,150 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +LslidarCH120Parser::LslidarCH120Parser(const Config& config) + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) { + last_packet_time = 0; +} + +void LslidarCH120Parser::GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg) { + // allocate a point cloud with same time and frame ID as raw data + out_msg->mutable_header()->set_timestamp_sec(scan_msg->basetime() / + 1000000000.0); + out_msg->mutable_header()->set_module_name(scan_msg->header().module_name()); + out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id()); + out_msg->set_height(1); + out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0); + out_msg->mutable_header()->set_sequence_num( + scan_msg->header().sequence_num()); + gps_base_usec_ = scan_msg->basetime(); + + packets_size = scan_msg->firing_pkts_size(); + + for (size_t i = 0; i < packets_size; ++i) { + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); + last_time_stamp_ = out_msg->measurement_time(); + ADEBUG << "stamp: " << std::fixed << last_time_stamp_; + } + + if (out_msg->point().empty()) { + // we discard this pointcloud if empty + AERROR << "All points is NAN!Please check lslidar:" << config_.model(); + } + // set default width + out_msg->set_width(out_msg->point_size()); +} + +/** @brief convert raw packet to point cloud + * @param pkt raw packet to Unpack + * @param pc shared pointer to point cloud (points are appended) + */ +void LslidarCH120Parser::Unpack(int num, const LslidarPacket& pkt, + std::shared_ptr pc) { + float x, y, z; + uint64_t packet_end_time; + double z_sin_altitude = 0.0; + double z_cos_altitude = 0.0; + const RawPacket* raw = (const RawPacket*)pkt.data().c_str(); + time_last = 0; + + packet_end_time = pkt.stamp(); + + for (int point_idx1 = 0; point_idx1 < POINTS_PER_PACKET; point_idx1++) { + firings[point_idx1].vertical_line = raw->points[point_idx1].vertical_line; + two_bytes point_amuzith; + point_amuzith.bytes[0] = raw->points[point_idx1].azimuth_2; + point_amuzith.bytes[1] = raw->points[point_idx1].azimuth_1; + firings[point_idx1].azimuth = + static_cast(point_amuzith.uint * 0.01 * DEG_TO_RAD); + four_bytes point_distance; + point_distance.bytes[0] = raw->points[point_idx1].distance_3; + point_distance.bytes[1] = raw->points[point_idx1].distance_2; + point_distance.bytes[2] = raw->points[point_idx1].distance_1; + point_distance.bytes[3] = 0; + firings[point_idx1].distance = + static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; + firings[point_idx1].intensity = raw->points[point_idx1].intensity; + } + + for (int point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + LaserCorrection& corrections = + calibration_.laser_corrections_[firings[point_idx].vertical_line]; + + if (config_.calibration()) + firings[point_idx].distance = + firings[point_idx].distance + corrections.dist_correction; + + if (firings[point_idx].distance > config_.max_range() || + firings[point_idx].distance < config_.min_range()) + continue; + + z_sin_altitude = + sin((-13 + 0.167 * firings[point_idx].vertical_line) * DEG_TO_RAD); + z_cos_altitude = sqrt(1 - z_sin_altitude * z_sin_altitude); + x = firings[point_idx].distance * z_cos_altitude * + cos(firings[point_idx].azimuth); + y = firings[point_idx].distance * z_cos_altitude * + sin(firings[point_idx].azimuth); + z = firings[point_idx].distance * z_sin_altitude; + + // Compute the time of the point + uint64_t point_time = + packet_end_time - 1282 * (POINTS_PER_PACKET - point_idx - 1); + if (time_last < point_time && time_last > 0) { + point_time = time_last + 1282; + } + time_last = point_time; + + PointXYZIT* point = pc->add_point(); + point->set_timestamp(point_time); + point->set_intensity(firings[point_idx].intensity); + + if (config_.calibration()) { + ComputeCoords2(firings[point_idx].vertical_line, CH120, + firings[point_idx].distance, &corrections, + firings[point_idx].azimuth, point); + + } else { + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); + } + } + } +} + +void LslidarCH120Parser::Order(std::shared_ptr cloud) {} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCH128X1_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCH128X1_parser.cc new file mode 100644 index 00000000000..c35c66398c3 --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidarCH128X1_parser.cc @@ -0,0 +1,205 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +LslidarCH128X1Parser::LslidarCH128X1Parser(const Config& config) + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) {} + +void LslidarCH128X1Parser::GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg) { + // allocate a point cloud with same time and frame ID as raw data + out_msg->mutable_header()->set_timestamp_sec(scan_msg->basetime() / + 1000000000.0); + out_msg->mutable_header()->set_module_name(scan_msg->header().module_name()); + out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id()); + out_msg->set_height(1); + out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0); + out_msg->mutable_header()->set_sequence_num( + scan_msg->header().sequence_num()); + gps_base_usec_ = scan_msg->basetime(); + + const unsigned char* difop_ptr = + (const unsigned char*)scan_msg->difop_pkts(0).data().c_str(); + + two_bytes angle_1, angle_2, angle_3, angle_4; + angle_1.bytes[0] = difop_ptr[243]; + angle_1.bytes[1] = difop_ptr[242]; + prism_angle[0] = angle_1.uint * 0.01; + + angle_2.bytes[0] = difop_ptr[245]; + angle_2.bytes[1] = difop_ptr[244]; + prism_angle[1] = angle_2.uint * 0.01; + + angle_3.bytes[0] = difop_ptr[247]; + angle_3.bytes[1] = difop_ptr[246]; + prism_angle[2] = angle_3.uint * 0.01; + + angle_4.bytes[0] = difop_ptr[249]; + angle_4.bytes[1] = difop_ptr[248]; + prism_angle[3] = angle_4.uint * 0.01; + + packets_size = scan_msg->firing_pkts_size(); + + for (size_t i = 0; i < packets_size; ++i) { + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); + last_time_stamp_ = out_msg->measurement_time(); + ADEBUG << "stamp: " << std::fixed << last_time_stamp_; + } + + AINFO << "packets_size :" << packets_size; + if (out_msg->point().empty()) { + // we discard this pointcloud if empty + AERROR << "All points is NAN!Please check lslidar:" << config_.model(); + } + + // set default width + out_msg->set_width(out_msg->point_size()); +} + +/** @brief convert raw packet to point cloud + * @param pkt raw packet to Unpack + * @param pc shared pointer to point cloud (points are appended) + */ +void LslidarCH128X1Parser::Unpack(int num, const LslidarPacket& pkt, + std::shared_ptr pc) { + float x, y, z; + uint64_t point_time; + uint64_t packet_end_time; + double z_sin_altitude = 0.0; + double z_cos_altitude = 0.0; + double sinTheta_1[128] = {0}; + double cosTheta_1[128] = {0}; + double sinTheta_2[128] = {0}; + double cosTheta_2[128] = {0}; + + for (int i = 0; i < 128; i++) { + sinTheta_1[i] = sin(big_angle[i / 4] * M_PI / 180); + cosTheta_1[i] = cos(big_angle[i / 4] * M_PI / 180); + + if (abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6 && + abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6) { + sinTheta_2[i] = sin((i % 4) * (-0.17) * M_PI / 180); + cosTheta_2[i] = cos((i % 4) * (-0.17) * M_PI / 180); + } else { + sinTheta_2[i] = sin(prism_angle[i % 4] * M_PI / 180); + cosTheta_2[i] = cos(prism_angle[i % 4] * M_PI / 180); + } + } + + const RawPacket* raw = (const RawPacket*)pkt.data().c_str(); + + packet_end_time = pkt.stamp(); + current_packet_time = packet_end_time; + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + firings[point_idx].vertical_line = raw->points[point_idx].vertical_line; + two_bytes point_amuzith; + point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; + point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; + firings[point_idx].azimuth = + static_cast(point_amuzith.uint) * 0.01 * DEG_TO_RAD; + four_bytes point_distance; + point_distance.bytes[0] = raw->points[point_idx].distance_3; + point_distance.bytes[1] = raw->points[point_idx].distance_2; + point_distance.bytes[2] = raw->points[point_idx].distance_1; + point_distance.bytes[3] = 0; + firings[point_idx].distance = + static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; + firings[point_idx].intensity = raw->points[point_idx].intensity; + } + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + LaserCorrection& corrections = + calibration_.laser_corrections_[firings[point_idx].vertical_line]; + + if (config_.calibration()) + firings[point_idx].distance = + firings[point_idx].distance + corrections.dist_correction; + + if (firings[point_idx].distance > config_.max_range() || + firings[point_idx].distance < config_.min_range()) + continue; + + double _R_ = cosTheta_2[firings[point_idx].vertical_line] * + cosTheta_1[firings[point_idx].vertical_line] * + cos(firings[point_idx].azimuth / 2.0) - + sinTheta_2[firings[point_idx].vertical_line] * + sinTheta_1[firings[point_idx].vertical_line]; + + z_sin_altitude = sinTheta_1[firings[point_idx].vertical_line] + + 2 * _R_ * sinTheta_2[firings[point_idx].vertical_line]; + + z_cos_altitude = sqrt(1 - pow(z_sin_altitude, 2)); + x = firings[point_idx].distance * z_cos_altitude * + cos(firings[point_idx].azimuth); + y = firings[point_idx].distance * z_cos_altitude * + sin(firings[point_idx].azimuth); + z = firings[point_idx].distance * z_sin_altitude; + + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) + return; + + // Compute the time of the point + // modify + if (previous_packet_time > 1e-6) { + point_time = current_packet_time - + (POINTS_PER_PACKET - point_idx - 1) * + (current_packet_time - previous_packet_time) / + (POINTS_PER_PACKET); + } else { // fist packet + point_time = current_packet_time; + } + + PointXYZIT* point = pc->add_point(); + point->set_timestamp(point_time); + point->set_intensity(firings[point_idx].intensity); + + if (config_.calibration()) { + ComputeCoords2(firings[point_idx].vertical_line, CH128X1, + firings[point_idx].distance, &corrections, + firings[point_idx].azimuth, point); + + } else { + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); + } + } + } + previous_packet_time = current_packet_time; +} + +void LslidarCH128X1Parser::Order(std::shared_ptr cloud) {} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCH128_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCH128_parser.cc new file mode 100644 index 00000000000..e5dc64c14f1 --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidarCH128_parser.cc @@ -0,0 +1,187 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +LslidarCH128Parser::LslidarCH128Parser(const Config &config) + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) { + last_packet_time = 0; +} + +void LslidarCH128Parser::GeneratePointcloud( + const std::shared_ptr &scan_msg, + const std::shared_ptr &out_msg) { + // allocate a point cloud with same time and frame ID as raw data + out_msg->mutable_header()->set_timestamp_sec(scan_msg->basetime() / + 1000000000.0); + out_msg->mutable_header()->set_module_name(scan_msg->header().module_name()); + out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id()); + out_msg->set_height(1); + out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0); + out_msg->mutable_header()->set_sequence_num( + scan_msg->header().sequence_num()); + gps_base_usec_ = scan_msg->basetime(); + + const unsigned char *difop_ptr = + (const unsigned char *)scan_msg->difop_pkts(0).data().c_str(); + + two_bytes angle_1, angle_2, angle_3, angle_4; + angle_1.bytes[0] = difop_ptr[87]; + angle_1.bytes[1] = difop_ptr[86]; + prism_angle128[0] = angle_1.uint * 0.01; + + angle_2.bytes[0] = difop_ptr[89]; + angle_2.bytes[1] = difop_ptr[88]; + prism_angle128[1] = angle_2.uint * 0.01; + + angle_3.bytes[0] = difop_ptr[91]; + angle_3.bytes[1] = difop_ptr[90]; + prism_angle128[2] = angle_3.uint * 0.01; + + angle_4.bytes[0] = difop_ptr[93]; + angle_4.bytes[1] = difop_ptr[92]; + prism_angle128[3] = angle_4.uint * 0.01; + + packets_size = scan_msg->firing_pkts_size(); + + for (size_t i = 0; i < packets_size; ++i) { + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); + last_time_stamp_ = out_msg->measurement_time(); + ADEBUG << "stamp: " << std::fixed << last_time_stamp_; + } + + if (out_msg->point().empty()) { + // we discard this pointcloud if empty + AERROR << "All points is NAN!Please check lslidar:" << config_.model(); + } + // set default width + out_msg->set_width(out_msg->point_size()); +} + +/** @brief convert raw packet to point cloud + * @param pkt raw packet to Unpack + * @param pc shared pointer to point cloud (points are appended) + */ +void LslidarCH128Parser::Unpack(int num, const LslidarPacket &pkt, + std::shared_ptr pc) { + float x, y, z; + uint64_t packet_end_time; + double z_sin_altitude = 0.0; + double z_cos_altitude = 0.0; + double cos_azimuth_half = 0.0; + const RawPacket *raw = (const RawPacket *)pkt.data().c_str(); + time_last = 0; + + packet_end_time = pkt.stamp(); + + for (int i = 0; i < 128; i++) { + if (abs(prism_angle128[0]) < 1e-6 && abs(prism_angle128[1]) < 1e-6 && + abs(prism_angle128[2]) < 1e-6 && abs(prism_angle128[3]) < 1e-6) { + sinTheta_2[i] = sin_scan_mirror_altitude_ch128[i % 4]; + } else { + sinTheta_2[i] = std::sin(prism_angle128[i % 4] * M_PI / 180); + } + } + for (int point_idx1 = 0; point_idx1 < POINTS_PER_PACKET; point_idx1++) { + firings[point_idx1].vertical_line = raw->points[point_idx1].vertical_line; + two_bytes point_amuzith; + point_amuzith.bytes[0] = raw->points[point_idx1].azimuth_2; + point_amuzith.bytes[1] = raw->points[point_idx1].azimuth_1; + firings[point_idx1].azimuth = + static_cast(point_amuzith.uint * 0.01 * DEG_TO_RAD); + four_bytes point_distance; + point_distance.bytes[0] = raw->points[point_idx1].distance_3; + point_distance.bytes[1] = raw->points[point_idx1].distance_2; + point_distance.bytes[2] = raw->points[point_idx1].distance_1; + point_distance.bytes[3] = 0; + firings[point_idx1].distance = + static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; + firings[point_idx1].intensity = raw->points[point_idx1].intensity; + } + + for (int point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + LaserCorrection &corrections = + calibration_.laser_corrections_[firings[point_idx].vertical_line]; + + if (config_.calibration()) + firings[point_idx].distance = + firings[point_idx].distance + corrections.dist_correction; + + if (firings[point_idx].distance > config_.max_range() || + firings[point_idx].distance < config_.min_range()) + continue; + + if (firings[point_idx].vertical_line / 4 % 2 == 0) { + cos_azimuth_half = + sin((firings[point_idx].azimuth - 15 * DEG_TO_RAD) * 0.5); + } else { + cos_azimuth_half = + cos((firings[point_idx].azimuth + 15 * DEG_TO_RAD) * 0.5); + } + + z_sin_altitude = + sin_scan_laser_altitude_ch128[firings[point_idx].vertical_line / 4] + + 2 * cos_azimuth_half * sinTheta_2[firings[point_idx].vertical_line]; + z_cos_altitude = sqrt(1 - z_sin_altitude * z_sin_altitude); + x = firings[point_idx].distance * z_cos_altitude * + cos(firings[point_idx].azimuth); + y = firings[point_idx].distance * z_cos_altitude * + sin(firings[point_idx].azimuth); + z = firings[point_idx].distance * z_sin_altitude; + + // Compute the time of the point + uint64_t point_time = + packet_end_time - 1562.5 * ((POINTS_PER_PACKET - point_idx) / 7 - 1); + if (time_last < point_time && time_last > 0) { + point_time = time_last + 1562.5; + } + time_last = point_time; + + PointXYZIT *point = pc->add_point(); + point->set_timestamp(point_time); + point->set_intensity(firings[point_idx].intensity); + + if (config_.calibration()) { + ComputeCoords2(firings[point_idx].vertical_line, CH128, + firings[point_idx].distance, &corrections, + firings[point_idx].azimuth, point); + } else { + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); + } + } + } +} + +void LslidarCH128Parser::Order(std::shared_ptr cloud) {} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCH16_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCH16_parser.cc new file mode 100644 index 00000000000..dc1df379ab1 --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidarCH16_parser.cc @@ -0,0 +1,203 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +LslidarCH16Parser::LslidarCH16Parser(const Config& config) + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) {} + +void LslidarCH16Parser::GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg) { + // allocate a point cloud with same time and frame ID as raw data + out_msg->mutable_header()->set_timestamp_sec(scan_msg->basetime() / + 1000000000.0); + out_msg->mutable_header()->set_module_name(scan_msg->header().module_name()); + out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id()); + out_msg->set_height(1); + out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0); + out_msg->mutable_header()->set_sequence_num( + scan_msg->header().sequence_num()); + gps_base_usec_ = scan_msg->basetime(); + + const unsigned char* difop_ptr = + (const unsigned char*)scan_msg->difop_pkts(0).data().c_str(); + + two_bytes angle_1, angle_2, angle_3, angle_4; + angle_1.bytes[0] = difop_ptr[663]; + angle_1.bytes[1] = difop_ptr[662]; + prism_angle[0] = angle_1.uint * 0.01; + + angle_2.bytes[0] = difop_ptr[665]; + angle_2.bytes[1] = difop_ptr[664]; + prism_angle[1] = angle_2.uint * 0.01; + + angle_3.bytes[0] = difop_ptr[667]; + angle_3.bytes[1] = difop_ptr[666]; + prism_angle[2] = angle_3.uint * 0.01; + + angle_4.bytes[0] = difop_ptr[669]; + angle_4.bytes[1] = difop_ptr[668]; + prism_angle[3] = angle_4.uint * 0.01; + packets_size = scan_msg->firing_pkts_size(); + + for (size_t i = 0; i < packets_size; ++i) { + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); + last_time_stamp_ = out_msg->measurement_time(); + ADEBUG << "stamp: " << std::fixed << last_time_stamp_; + } + + if (out_msg->point().empty()) { + // we discard this pointcloud if empty + AERROR << "All points is NAN!Please check lslidar:" << config_.model(); + } + + // set default width + out_msg->set_width(out_msg->point_size()); +} + +/** @brief convert raw packet to point cloud + * @param pkt raw packet to Unpack + * @param pc shared pointer to point cloud (points are appended) + */ +void LslidarCH16Parser::Unpack(int num, const LslidarPacket& pkt, + std::shared_ptr pc) { + float x, y, z; + uint64_t packet_end_time; + double z_sin_altitude = 0.0; + double z_cos_altitude = 0.0; + time_last = 0; + const RawPacket* raw = (const RawPacket*)pkt.data().c_str(); + + packet_end_time = pkt.stamp(); + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + firings[point_idx].vertical_line = raw->points[point_idx].vertical_line; + two_bytes point_amuzith; + point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; + point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; + firings[point_idx].azimuth = + static_cast(point_amuzith.uint) * 0.01 * DEG_TO_RAD; + four_bytes point_distance; + point_distance.bytes[0] = raw->points[point_idx].distance_3; + point_distance.bytes[1] = raw->points[point_idx].distance_2; + point_distance.bytes[2] = raw->points[point_idx].distance_1; + point_distance.bytes[3] = 0; + firings[point_idx].distance = + static_cast(point_distance.uint) / 256.0 * DISTANCE_RESOLUTION; + firings[point_idx].intensity = raw->points[point_idx].intensity; + } + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + LaserCorrection& corrections = + calibration_.laser_corrections_[firings[point_idx].vertical_line]; + + if (config_.calibration()) + firings[point_idx].distance = + firings[point_idx].distance + corrections.dist_correction; + + if (firings[point_idx].distance > config_.max_range() || + firings[point_idx].distance < config_.min_range()) + continue; + + // Convert the point to xyz coordinate + double cos_azimuth_half = cos(firings[point_idx].azimuth * 0.5); + + if (abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6 && + abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6) { + z_sin_altitude = + sin_scan_laser_altitude[firings[point_idx].vertical_line / 4 + 2] + + 2 * cos_azimuth_half * + sin_scan_mirror_altitude[firings[point_idx].vertical_line % 4]; + } else { + z_sin_altitude = + sin_scan_laser_altitude[firings[point_idx].vertical_line / 4 + 2] + + 2 * cos_azimuth_half * + sin(prism_angle[firings[point_idx].vertical_line % 4] * M_PI / + 180); + } + + z_cos_altitude = sqrt(1 - z_sin_altitude * z_sin_altitude); + + x = firings[point_idx].distance * z_cos_altitude * + cos(firings[point_idx].azimuth); + y = firings[point_idx].distance * z_cos_altitude * + sin(firings[point_idx].azimuth); + z = firings[point_idx].distance * z_sin_altitude; + + // Compute the time of the point + uint64_t point_time = + packet_end_time - 1665 * (POINTS_PER_PACKET - point_idx - 1); + if (time_last < point_time && time_last > 0) { + point_time = time_last + 1665; + } + time_last = point_time; + + PointXYZIT* point = pc->add_point(); + point->set_timestamp(point_time / 1000000000.0); + point->set_intensity(firings[point_idx].intensity); + + if (config_.calibration()) { + ComputeCoords2(firings[point_idx].vertical_line, CH16, + firings[point_idx].distance, &corrections, + firings[point_idx].azimuth, point); + } else { + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); + } + } + } +} + +void LslidarCH16Parser::Order(std::shared_ptr cloud) { + int width = 16; + cloud->set_width(width); + int height = cloud->point_size() / cloud->width(); + cloud->set_height(height); + + std::shared_ptr cloud_origin = std::make_shared(); + cloud_origin->CopyFrom(*cloud); + + for (int i = 0; i < width; ++i) { + int col = lslidar::ORDER_32[i]; + + for (int j = 0; j < height; ++j) { + // make sure offset is initialized, should be init at setup() just once + int target_index = j * width + i; + int origin_index = j * width + col; + cloud->mutable_point(target_index) + ->CopyFrom(cloud_origin->point(origin_index)); + } + } +} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCH32_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCH32_parser.cc new file mode 100644 index 00000000000..d5e6cc4f044 --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidarCH32_parser.cc @@ -0,0 +1,182 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +LslidarCH32Parser::LslidarCH32Parser(const Config &config) + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) {} + +void LslidarCH32Parser::GeneratePointcloud( + const std::shared_ptr &scan_msg, + const std::shared_ptr &out_msg) { + // allocate a point cloud with same time and frame ID as raw data + out_msg->mutable_header()->set_timestamp_sec(scan_msg->basetime() / + 1000000000.0); + out_msg->mutable_header()->set_module_name(scan_msg->header().module_name()); + out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id()); + out_msg->set_height(1); + out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0); + out_msg->mutable_header()->set_sequence_num( + scan_msg->header().sequence_num()); + gps_base_usec_ = scan_msg->basetime(); + + packets_size = scan_msg->firing_pkts_size(); + + for (size_t i = 0; i < packets_size; ++i) { + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); + last_time_stamp_ = out_msg->measurement_time(); + ADEBUG << "stamp: " << std::fixed << last_time_stamp_; + } + + if (out_msg->point().empty()) { + // we discard this pointcloud if empty + AERROR << "All points is NAN!Please check lslidar:" << config_.model(); + } + + // set default width + out_msg->set_width(out_msg->point_size()); +} + +/** @brief convert raw packet to point cloud + * @param pkt raw packet to Unpack + * @param pc shared pointer to point cloud (points are appended) + */ +void LslidarCH32Parser::Unpack(int num, const LslidarPacket &pkt, + std::shared_ptr pc) { + float x, y, z; + uint64_t packet_end_time; + double z_sin_altitude = 0.0; + double z_cos_altitude = 0.0; + time_last = 0; + const RawPacket *raw = (const RawPacket *)pkt.data().c_str(); + + packet_end_time = pkt.stamp(); + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + firings[point_idx].vertical_line = raw->points[point_idx].vertical_line; + two_bytes point_amuzith; + point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; + point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; + firings[point_idx].azimuth = + static_cast(point_amuzith.uint) * 0.01 * DEG_TO_RAD; + four_bytes point_distance; + point_distance.bytes[0] = raw->points[point_idx].distance_3; + point_distance.bytes[1] = raw->points[point_idx].distance_2; + point_distance.bytes[2] = raw->points[point_idx].distance_1; + point_distance.bytes[3] = 0; + firings[point_idx].distance = + static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; + firings[point_idx].intensity = raw->points[point_idx].intensity; + } + + double scan_laser_altitude_degree[8]; + if (1 == config_.degree_mode()) { + for (int i = 0; i < 8; i++) + scan_laser_altitude_degree[i] = sin_scan_laser_altitude[i]; + } else { + for (int i = 0; i < 8; i++) + scan_laser_altitude_degree[i] = sin_scan_laser_altitude_ch32[i]; + } + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + LaserCorrection &corrections = + calibration_.laser_corrections_[firings[point_idx].vertical_line]; + + if (config_.calibration()) + firings[point_idx].distance = + firings[point_idx].distance + corrections.dist_correction; + + if (firings[point_idx].distance > config_.max_range() || + firings[point_idx].distance < config_.min_range()) + continue; + + // Convert the point to xyz coordinate + double cos_azimuth_half = cos(firings[point_idx].azimuth * 0.5); + z_sin_altitude = + scan_laser_altitude_degree[firings[point_idx].vertical_line / 4] + + 2 * cos_azimuth_half * + sin_scan_mirror_altitude[firings[point_idx].vertical_line % 4]; + z_cos_altitude = sqrt(1 - z_sin_altitude * z_sin_altitude); + + x = firings[point_idx].distance * z_cos_altitude * + cos(firings[point_idx].azimuth); + y = firings[point_idx].distance * z_cos_altitude * + sin(firings[point_idx].azimuth); + z = firings[point_idx].distance * z_sin_altitude; + + // Compute the time of the point + uint64_t point_time = + packet_end_time - 1665 * (POINTS_PER_PACKET - point_idx - 1); + if (time_last < point_time && time_last > 0) { + point_time = time_last + 1665; + } + time_last = point_time; + + PointXYZIT *point = pc->add_point(); + (point_time / 1000000000.0); + point->set_intensity(firings[point_idx].intensity); + + if (config_.calibration()) { + ComputeCoords2(firings[point_idx].vertical_line, CH32, + firings[point_idx].distance, &corrections, + firings[point_idx].azimuth, point); + } else { + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); + } + } + } +} + +void LslidarCH32Parser::Order(std::shared_ptr cloud) { + int width = 32; + cloud->set_width(width); + int height = cloud->point_size() / cloud->width(); + cloud->set_height(height); + + std::shared_ptr cloud_origin = std::make_shared(); + cloud_origin->CopyFrom(*cloud); + + for (int i = 0; i < width; ++i) { + int col = lslidar::ORDER_32[i]; + + for (int j = 0; j < height; ++j) { + // make sure offset is initialized, should be init at setup() just once + int target_index = j * width + i; + int origin_index = j * width + col; + cloud->mutable_point(target_index) + ->CopyFrom(cloud_origin->point(origin_index)); + } + } +} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCH64_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCH64_parser.cc new file mode 100644 index 00000000000..a040cdd0ef5 --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidarCH64_parser.cc @@ -0,0 +1,165 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +LslidarCH64Parser::LslidarCH64Parser(const Config& config) + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) {} + +void LslidarCH64Parser::GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg) { + // allocate a point cloud with same time and frame ID as raw data + out_msg->mutable_header()->set_timestamp_sec(scan_msg->basetime() / + 1000000000.0); + out_msg->mutable_header()->set_module_name(scan_msg->header().module_name()); + out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id()); + out_msg->set_height(1); + out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0); + out_msg->mutable_header()->set_sequence_num( + scan_msg->header().sequence_num()); + gps_base_usec_ = scan_msg->basetime(); + + packets_size = scan_msg->firing_pkts_size(); + + for (size_t i = 0; i < packets_size; ++i) { + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); + last_time_stamp_ = out_msg->measurement_time(); + ADEBUG << "stamp: " << std::fixed << last_time_stamp_; + } + + if (out_msg->point().empty()) { + // we discard this pointcloud if empty + AERROR << "All points is NAN!Please check lslidar:" << config_.model(); + } + + // set default width + out_msg->set_width(out_msg->point_size()); +} + +/** @brief convert raw packet to point cloud + * @param pkt raw packet to Unpack + * @param pc shared pointer to point cloud (points are appended) + */ +void LslidarCH64Parser::Unpack(int num, const LslidarPacket& pkt, + std::shared_ptr pc) { + float x, y, z; + uint64_t point_time; + uint64_t packet_end_time; + double z_sin_altitude = 0.0; + double z_cos_altitude = 0.0; + time_last = 0; + const RawPacket* raw = (const RawPacket*)pkt.data().c_str(); + + packet_end_time = pkt.stamp(); + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + firings[point_idx].vertical_line = raw->points[point_idx].vertical_line; + two_bytes point_amuzith; + point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; + point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; + firings[point_idx].azimuth = + static_cast(point_amuzith.uint) * 0.01 * DEG_TO_RAD; + four_bytes point_distance; + point_distance.bytes[0] = raw->points[point_idx].distance_3; + point_distance.bytes[1] = raw->points[point_idx].distance_2; + point_distance.bytes[2] = raw->points[point_idx].distance_1; + point_distance.bytes[3] = 0; + firings[point_idx].distance = + static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; + firings[point_idx].intensity = raw->points[point_idx].intensity; + } + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + LaserCorrection& corrections = + calibration_.laser_corrections_[firings[point_idx].vertical_line]; + + if (config_.calibration()) + firings[point_idx].distance = + firings[point_idx].distance + corrections.dist_correction; + + if (firings[point_idx].distance > config_.max_range() || + firings[point_idx].distance < config_.min_range()) + continue; + + int line_num = firings[point_idx].vertical_line; + + // Convert the point to xyz coordinate + if (line_num % 8 == 0 || line_num % 8 == 1 || line_num % 8 == 2 || + line_num % 8 == 3) { + z_sin_altitude = + sin(-13.33 * DEG_TO_RAD + floor(line_num / 4) * 1.33 * DEG_TO_RAD) + + 2 * cos(firings[point_idx].azimuth / 2 + 1.05 * DEG_TO_RAD) * + sin((line_num % 4) * 0.33 * DEG_TO_RAD); + + } else if (line_num % 8 == 4 || line_num % 8 == 5 || line_num % 8 == 6 || + line_num % 8 == 7) { + z_sin_altitude = + sin(-13.33 * DEG_TO_RAD + floor(line_num / 4) * 1.33 * DEG_TO_RAD) + + 2 * cos(firings[point_idx].azimuth / 2 - 1.05 * DEG_TO_RAD) * + sin((line_num % 4) * 0.33 * DEG_TO_RAD); + } + + z_cos_altitude = sqrt(1 - z_sin_altitude * z_sin_altitude); + x = firings[point_idx].distance * z_cos_altitude * + cos(firings[point_idx].azimuth); + y = firings[point_idx].distance * z_cos_altitude * + sin(firings[point_idx].azimuth); + z = firings[point_idx].distance * z_sin_altitude; + + // Compute the time of the point + point_time = packet_end_time - 1726 * (POINTS_PER_PACKET - point_idx - 1); + if (time_last < point_time && time_last > 0) { + point_time = time_last + 1726; + } + time_last = point_time; + + PointXYZIT* point = pc->add_point(); + point->set_timestamp(point_time); + point->set_intensity(firings[point_idx].intensity); + + if (config_.calibration()) { + ComputeCoords2(firings[point_idx].vertical_line, CH64, + firings[point_idx].distance, &corrections, + firings[point_idx].azimuth, point); + + } else { + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); + } + } + } +} + +void LslidarCH64Parser::Order(std::shared_ptr cloud) {} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCH64w_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCH64w_parser.cc new file mode 100644 index 00000000000..10b5b9d584f --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidarCH64w_parser.cc @@ -0,0 +1,204 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +LslidarCH64wParser::LslidarCH64wParser(const Config& config) + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) { + point_time_diff = 1 / (60.0 * 1000 * 32); // s + for (int i = 0; i < 4; ++i) { + prism_angle64[i] = i * 0.35; + } + for (int j = 0; j < 128; ++j) { + if (j / 4 % 2 == 0) { + theat1_s[j] = sin((-25 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180); + theat1_c[j] = cos((-25 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180); + } else { + theat1_s[j] = sin((-24 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180); + theat1_c[j] = cos((-24 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180); + } + } +} + +void LslidarCH64wParser::GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg) { + // allocate a point cloud with same time and frame ID as raw data + out_msg->mutable_header()->set_timestamp_sec(scan_msg->basetime() / + 1000000000.0); + out_msg->mutable_header()->set_module_name(scan_msg->header().module_name()); + out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id()); + out_msg->set_height(1); + out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0); + out_msg->mutable_header()->set_sequence_num( + scan_msg->header().sequence_num()); + gps_base_usec_ = scan_msg->basetime(); + + packets_size = scan_msg->firing_pkts_size(); + AINFO << "packets_size :" << packets_size; + for (size_t i = 0; i < packets_size; ++i) { + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); + last_time_stamp_ = out_msg->measurement_time(); + ADEBUG << "stamp: " << std::fixed << last_time_stamp_; + } + if (out_msg->point().empty()) { + // we discard this pointcloud if empty + AERROR << "All points is NAN!Please check lslidar:" << config_.model(); + } + + // set default width + out_msg->set_width(out_msg->point_size()); +} + +/** @brief convert raw packet to point cloud + * @param pkt raw packet to Unpack + * @param pc shared pointer to point cloud (points are appended) + */ +void LslidarCH64wParser::Unpack(int num, const LslidarPacket& pkt, + std::shared_ptr pc) { + float x, y, z; + uint64_t point_time; + uint64_t packet_end_time; + time_last = 0; + const RawPacket* raw = (const RawPacket*)pkt.data().c_str(); + double cos_xita; + // 垂直角度 + double sin_theat; + double cos_theat; + double _R_; + // 水平角度 + double cos_H_xita; + double sin_H_xita; + double cos_xita_F; + double sin_xita_F; + + packet_end_time = pkt.stamp(); + + current_packet_time = packet_end_time; + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + firings[point_idx].vertical_line = raw->points[point_idx].vertical_line; + two_bytes point_amuzith; + point_amuzith.bytes[0] = raw->points[point_idx].azimuth_2; + point_amuzith.bytes[1] = raw->points[point_idx].azimuth_1; + firings[point_idx].azimuth = + static_cast(point_amuzith.uint) * 0.01 * DEG_TO_RAD; + four_bytes point_distance; + point_distance.bytes[0] = raw->points[point_idx].distance_3; + point_distance.bytes[1] = raw->points[point_idx].distance_2; + point_distance.bytes[2] = raw->points[point_idx].distance_1; + point_distance.bytes[3] = 0; + firings[point_idx].distance = + static_cast(point_distance.uint) * DISTANCE_RESOLUTION2; + firings[point_idx].intensity = raw->points[point_idx].intensity; + } + + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET; point_idx++) { + LaserCorrection& corrections = + calibration_.laser_corrections_[firings[point_idx].vertical_line]; + + if (config_.calibration()) + firings[point_idx].distance = + firings[point_idx].distance + corrections.dist_correction; + + if (firings[point_idx].distance > config_.max_range() || + firings[point_idx].distance < config_.min_range()) + continue; + + int line_num = firings[point_idx].vertical_line; + if (line_num / 4 % 2 == 0) { + cos_xita = cos((firings[point_idx].azimuth * RAD_TO_DEG / 2.0 + 22.5) * + M_PI / 180); + } else { + cos_xita = cos((-firings[point_idx].azimuth * RAD_TO_DEG / 2.0 + 112.5) * + M_PI / 180); + } + + _R_ = theat2_c[line_num] * theat1_c[line_num] * cos_xita - + theat2_s[line_num] * theat1_s[line_num]; + sin_theat = theat1_s[line_num] + 2 * _R_ * theat2_s[line_num]; + cos_theat = sqrt(1 - pow(sin_theat, 2)); + cos_H_xita = + (2 * _R_ * theat2_c[line_num] * cos_xita - theat1_c[line_num]) / + cos_theat; + sin_H_xita = sqrt(1 - pow(cos_H_xita, 2)); + + if (line_num / 4 % 2 == 0) { + cos_xita_F = (cos_H_xita + sin_H_xita) * sqrt(0.5); + sin_xita_F = sqrt(1 - pow(cos_xita_F, 2)); + } else { + cos_xita_F = (cos_H_xita + sin_H_xita) * (-sqrt(0.5)); + sin_xita_F = sqrt(1 - pow(cos_xita_F, 2)); + } + + x = firings[point_idx].distance * cos_theat * cos_xita_F; + y = firings[point_idx].distance * cos_theat * sin_xita_F; + z = firings[point_idx].distance * sin_theat; + + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) + return; + // modify + if (previous_packet_time > 1e-6) { + point_time = current_packet_time - + (POINTS_PER_PACKET - point_idx - 1) * + (current_packet_time - previous_packet_time) / + (POINTS_PER_PACKET); + } else { // fist packet + point_time = current_packet_time; + } + + PointXYZIT* point = pc->add_point(); + point->set_timestamp(point_time); + point->set_intensity(firings[point_idx].intensity); + + if (config_.calibration()) { + ComputeCoords2(firings[point_idx].vertical_line, CH64w, + firings[point_idx].distance, &corrections, + firings[point_idx].azimuth, point); + + } else { + if ((y >= config_.bottom_left_x() && y <= config_.top_right_x()) && + (-x >= config_.bottom_left_y() && -x <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y); + point->set_y(-x); + point->set_z(z); + } + } + } + previous_packet_time = current_packet_time; +} + +void LslidarCH64wParser::Order(std::shared_ptr cloud) {} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidarCXV4_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarCXV4_parser.cc new file mode 100644 index 00000000000..25da56b8cb3 --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidarCXV4_parser.cc @@ -0,0 +1,362 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +LslidarCXV4Parser::LslidarCXV4Parser(const Config &config) + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) { + vertical_angle_ = config.vertical_angle(); + distance_unit_ = config.distance_unit(); + is_new_c32w_ = true; + is_get_scan_altitude_ = false; + AERROR << "vertical_angle_ " << vertical_angle_; + if (config_.model() == LSLIDAR_C16_V4) { + lidar_number_ = 16; + AERROR << "lidar: c16"; + for (int i = 0; i < 16; ++i) { + sin_scan_altitude[i] = sin(c16_vertical_angle[i] * DEG_TO_RAD); + cos_scan_altitude[i] = cos(c16_vertical_angle[i] * DEG_TO_RAD); + } + } else if (config_.model() == LSLIDAR_C8_V4) { + lidar_number_ = 8; + AERROR << "lidar: c8"; + for (int i = 0; i < 8; ++i) { + sin_scan_altitude[i] = sin(c8_vertical_angle[i] * DEG_TO_RAD); + cos_scan_altitude[i] = cos(c8_vertical_angle[i] * DEG_TO_RAD); + } + } else if (config_.model() == LSLIDAR_C1_V4) { + lidar_number_ = 1; + AERROR << "lidar: c1"; + for (int i = 0; i < 8; ++i) { + sin_scan_altitude[i] = sin(c1_vertical_angle[i] * DEG_TO_RAD); + cos_scan_altitude[i] = cos(c1_vertical_angle[i] * DEG_TO_RAD); + } + } else if (config_.model() == LSLIDAR_C32_V4) { + lidar_number_ = 32; + if (32 == config.vertical_angle()) { + AERROR << "Vertical angle: 32 degrees"; + for (int i = 0; i < 32; ++i) { + sin_scan_altitude[i] = sin(c32_32_vertical_angle[i] * DEG_TO_RAD); + cos_scan_altitude[i] = cos(c32_32_vertical_angle[i] * DEG_TO_RAD); + } + } else if (70 == config.vertical_angle()) { + AERROR << "Vertical angle: 70 degrees"; + for (int k = 0; k < 32; ++k) { + sin_scan_altitude[k] = sin(c32_70_vertical_angle2[k] * DEG_TO_RAD); + cos_scan_altitude[k] = cos(c32_70_vertical_angle2[k] * DEG_TO_RAD); + } + } else if (90 == config.vertical_angle()) { + AERROR << "Vertical angle: 90 degrees"; + for (int k = 0; k < 32; ++k) { + sin_scan_altitude[k] = sin(c32_90_vertical_angle[k] * DEG_TO_RAD); + cos_scan_altitude[k] = cos(c32_90_vertical_angle[k] * DEG_TO_RAD); + } + } + } + + // create the sin and cos table for different azimuth values + for (int j = 0; j < 36000; ++j) { + double angle = static_cast(j) / 100.0 * DEG_TO_RAD; + sin_azimuth_table[j] = sin(angle); + cos_azimuth_table[j] = cos(angle); + } + config_vert_angle = false; + lslidar_type = 2; +} + +void LslidarCXV4Parser::decodePacket(const RawPacket_C32 *packet) { + // couputer azimuth angle for each firing, 12 + for (size_t b_idx = 0; b_idx < BLOCKS_PER_PACKET; ++b_idx) { + firings.firing_azimuth[b_idx] = + packet->blocks[b_idx].rotation % 36000; //* 0.01 * DEG_TO_RAD; + } + for (size_t block_idx = 0; block_idx < BLOCKS_PER_PACKET; ++block_idx) { + const RawBlock &raw_block = packet->blocks[block_idx]; + + int32_t azimuth_diff_b = 0; + if (config_.return_mode() == 1) { + if (block_idx < BLOCKS_PER_PACKET - 1) { + azimuth_diff_b = firings.firing_azimuth[block_idx + 1] - + firings.firing_azimuth[block_idx]; + azimuth_diff_b = + azimuth_diff_b < 0 ? azimuth_diff_b + 36000 : azimuth_diff_b; + + } else { + azimuth_diff_b = firings.firing_azimuth[block_idx] - + firings.firing_azimuth[block_idx - 1]; + + azimuth_diff_b = + azimuth_diff_b < 0 ? azimuth_diff_b + 36000 : azimuth_diff_b; + } + } else { + // return mode 2 + if (block_idx < BLOCKS_PER_PACKET - 2) { + azimuth_diff_b = firings.firing_azimuth[block_idx + 2] - + firings.firing_azimuth[block_idx]; + azimuth_diff_b = + azimuth_diff_b < 0 ? azimuth_diff_b + 36000 : azimuth_diff_b; + + } else { + azimuth_diff_b = firings.firing_azimuth[block_idx] - + firings.firing_azimuth[block_idx - 2]; + + azimuth_diff_b = + azimuth_diff_b < 0 ? azimuth_diff_b + 36000 : azimuth_diff_b; + } + } + + // 32 scan + for (size_t scan_fir_idx = 0; scan_fir_idx < SCANS_PER_FIRING_C32; + ++scan_fir_idx) { + size_t byte_idx = RAW_SCAN_SIZE * scan_fir_idx; + // azimuth + firings.azimuth[block_idx * 32 + scan_fir_idx] = + firings.firing_azimuth[block_idx] + + scan_fir_idx * azimuth_diff_b / FIRING_TOFFSET_C32; + firings.azimuth[block_idx * 32 + scan_fir_idx] = + firings.azimuth[block_idx * 32 + scan_fir_idx] % 36000; + // distance + TwoBytes raw_distance{}; + raw_distance.bytes[0] = raw_block.data[byte_idx]; + raw_distance.bytes[1] = raw_block.data[byte_idx + 1]; + firings.distance[block_idx * 32 + scan_fir_idx] = + static_cast(raw_distance.distance) * DISTANCE_RESOLUTION * + distance_unit_; + + // intensity + firings.intensity[block_idx * 32 + scan_fir_idx] = + static_cast(raw_block.data[byte_idx + 2]); + } + } + return; +} + +bool LslidarCXV4Parser::checkPacketValidity(const RawPacket_C32 *packet) { + for (size_t blk_idx = 0; blk_idx < BLOCKS_PER_PACKET; ++blk_idx) { + if (packet->blocks[blk_idx].header != UPPER_BANK) { + return false; + } + } + return true; +} + +void LslidarCXV4Parser::GeneratePointcloud( + const std::shared_ptr &scan_msg, + const std::shared_ptr &out_msg) { + // allocate a point cloud with same time and frame ID as raw data + out_msg->mutable_header()->set_timestamp_sec(scan_msg->basetime() / + 1000000000.0); + out_msg->mutable_header()->set_module_name(scan_msg->header().module_name()); + out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id()); + out_msg->set_height(1); + out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0); + out_msg->mutable_header()->set_sequence_num( + scan_msg->header().sequence_num()); + gps_base_usec_ = scan_msg->basetime(); + + const unsigned char *difop_ptr = + (const unsigned char *)scan_msg->difop_pkts(0).data().c_str(); + if (difop_ptr[0] == 0xa5 && difop_ptr[1] == 0xff && difop_ptr[2] == 0x00 && + difop_ptr[3] == 0x5a) { + AINFO << "设备包，暂时用不上..."; // todo 暂时不用设备包 + } + + size_t packets_size = scan_msg->firing_pkts_size(); + block_num = 0; + packet_number_ = packets_size; + + AINFO << "packets_size :" << packets_size; + + for (size_t i = 0; i < packets_size; ++i) { + Unpack(scan_msg->firing_pkts(static_cast(i)), out_msg, i); + last_time_stamp_ = out_msg->measurement_time(); + ADEBUG << "stamp: " << std::fixed << last_time_stamp_; + } + + if (out_msg->point().empty()) { + // we discard this pointcloud if empty + AERROR << "All points is NAN!Please check lslidar:" << config_.model(); + } + + // set default width + out_msg->set_width(out_msg->point_size()); +} + +/** @brief convert raw packet to point cloud + * @param pkt raw packet to Unpack + * @param pc shared pointer to point cloud (points are appended) + */ +void LslidarCXV4Parser::Unpack(const LslidarPacket &pkt, + std::shared_ptr pc, + int packet_number) { + double point_time; + const RawPacket_C32 *raw_packet = (const RawPacket_C32 *)(pkt.data().c_str()); + uint8_t *data = + reinterpret_cast(const_cast(pkt.data().c_str())); + + // check if the packet is valid + if (!checkPacketValidity(raw_packet)) return; + + // decode the packet + decodePacket(raw_packet); + + if (!is_get_scan_altitude_ && + static_cast(data[1211]) == 0x46) { // old C32w + AERROR << "byte[1211] == 0x46: old C32W"; + is_new_c32w_ = false; + for (int k = 0; k < 32; ++k) { + sin_scan_altitude[k] = sin(c32_70_vertical_angle[k] * DEG_TO_RAD); + cos_scan_altitude[k] = cos(c32_70_vertical_angle[k] * DEG_TO_RAD); + } + is_get_scan_altitude_ = true; + } + + for (size_t fir_idx = 0; fir_idx < SCANS_PER_PACKET; ++fir_idx) { + if (!is_new_c32w_ && vertical_angle_ == 70) { + if (fir_idx % 32 == 29 || fir_idx % 32 == 6 || fir_idx % 32 == 14 || + fir_idx % 32 == 22 || fir_idx % 32 == 30 || fir_idx % 32 == 7 || + fir_idx % 32 == 15 || fir_idx % 32 == 23) { + firings.azimuth[fir_idx] += 389; + } + if (firings.azimuth[fir_idx] > 36000) firings.azimuth[fir_idx] -= 36000; + } + // convert the point to xyz coordinate + size_t table_idx = firings.azimuth[fir_idx]; + double cos_azimuth = cos_azimuth_table[table_idx]; + double sin_azimuth = sin_azimuth_table[table_idx]; + double x_coord, y_coord, z_coord; + bool coordinate_opt = true; + double R1 = 0.0; + if (vertical_angle_ == 90) { // ch32r + R1 = CX4_R1_90; + } else if (is_new_c32w_) { // new C32w + R1 = CX4_C32W; + } else { // others + R1 = CX4_R1_; + } + double conversionAngle = + (vertical_angle_ == 90) ? CX4_conversionAngle_90 : CX4_conversionAngle_; + if (coordinate_opt) { + x_coord = firings.distance[fir_idx] * + cos_scan_altitude[fir_idx % lidar_number_] * cos_azimuth + + R1 * cos((conversionAngle - firings.azimuth[fir_idx] * 0.01) * + DEG_TO_RAD); + y_coord = -firings.distance[fir_idx] * + cos_scan_altitude[fir_idx % lidar_number_] * sin_azimuth + + R1 * sin((conversionAngle - firings.azimuth[fir_idx] * 0.01) * + DEG_TO_RAD); + z_coord = firings.distance[fir_idx] * + sin_scan_altitude[fir_idx % lidar_number_]; + + } else { + // Y-axis correspondence 0 degree + x_coord = firings.distance[fir_idx] * + cos_scan_altitude[fir_idx % lidar_number_] * sin_azimuth + + R1 * sin((firings.azimuth[fir_idx] * 0.01 - conversionAngle) * + DEG_TO_RAD); + y_coord = firings.distance[fir_idx] * + cos_scan_altitude[fir_idx % lidar_number_] * cos_azimuth + + R1 * cos((firings.azimuth[fir_idx] * 0.01 - conversionAngle) * + DEG_TO_RAD); + z_coord = firings.distance[fir_idx] * + sin_scan_altitude[fir_idx % lidar_number_]; + } + + // computer the time of the point + if (last_packet_time > 1e-6) { + point_time = current_packet_time - + (current_packet_time - previous_packet_time) * + (SCANS_PER_PACKET - fir_idx - 1) / SCANS_PER_PACKET; + } else { + point_time = current_packet_time; + } + + if (firings.distance[fir_idx] > config_.max_range() || + firings.distance[fir_idx] < config_.min_range()) { + PointXYZIT *point = pc->add_point(); + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + if ((firings.azimuth[fir_idx] < config_.scan_start_angle()) || + (firings.azimuth[fir_idx] > config_.scan_end_angle())) + continue; + if (packet_number == 0) { + if (firings.azimuth[fir_idx] > 30000) { + continue; + } + } + + if (packet_number == packet_number_ - 1) { + if (firings.azimuth[fir_idx] < 10000) { + continue; + } + } + + PointXYZIT *point = pc->add_point(); + point->set_timestamp(point_time); + + point->set_intensity(firings.intensity[fir_idx]); + if ((y_coord >= config_.bottom_left_x() && + y_coord <= config_.top_right_x()) && + (-x_coord >= config_.bottom_left_y() && + -x_coord <= config_.top_right_y())) { + point->set_x(nan); + point->set_y(nan); + point->set_z(nan); + point->set_timestamp(point_time); + point->set_intensity(0); + } else { + point->set_x(y_coord); + point->set_y(-x_coord); + point->set_z(z_coord); + } + } + } + previous_packet_time = current_packet_time; +} + +void LslidarCXV4Parser::Order(std::shared_ptr cloud) { + int width = 32; + cloud->set_width(width); + int height = cloud->point_size() / cloud->width(); + cloud->set_height(height); + + std::shared_ptr cloud_origin = std::make_shared(); + cloud_origin->CopyFrom(*cloud); + + for (int i = 0; i < width; ++i) { + int col = lslidar::ORDER_32[i]; + + for (int j = 0; j < height; ++j) { + // make sure offset is initialized, should be init at setup() just once + int target_index = j * width + i; + int origin_index = j * width + col; + cloud->mutable_point(target_index) + ->CopyFrom(cloud_origin->point(origin_index)); + } + } +} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidarLS128S2_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidarLS128S2_parser.cc new file mode 100644 index 00000000000..f0ced67767c --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidarLS128S2_parser.cc @@ -0,0 +1,469 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" + +namespace apollo { +namespace drivers { +namespace lslidar { +static float g_fDistanceAcc = 0.1 * 0.01; +static double cos30 = std::cos(DEG2RAD(30)); +static double sin30 = std::sin(DEG2RAD(30)); +static double sin60 = std::sin(DEG2RAD(60)); + +LslidarLS128S2Parser::LslidarLS128S2Parser(const Config &config) + : LslidarParser(config), previous_packet_stamp_(0), gps_base_usec_(0) { + // create the sin and cos table for different azimuth and vertical values + for (int j = 0; j < 36000; ++j) { + double angle = static_cast(j) / 100.0 * M_PI / 180.0; + sin_table[j] = sin(angle); + cos_table[j] = cos(angle); + } + + double mirror_angle[4] = {0, -2, -1, + -3}; // 摆镜角度 //根据通道不同偏移角度不同 + for (int i = 0; i < 4; ++i) { + cos_mirror_angle[i] = cos(DEG2RAD(mirror_angle[i])); + sin_mirror_angle[i] = sin(DEG2RAD(mirror_angle[i])); + } + cur_pc.reset(new PointCloud()); + pre_pc.reset(new PointCloud()); +} + +void LslidarLS128S2Parser::GeneratePointcloud( + const std::shared_ptr &scan_msg, + const std::shared_ptr &out_msg) { + // allocate a point cloud with same time and frame ID as raw data + out_msg->mutable_header()->set_timestamp_sec(scan_msg->basetime() / + 1000000000.0); + out_msg->mutable_header()->set_module_name(scan_msg->header().module_name()); + out_msg->mutable_header()->set_frame_id(scan_msg->header().frame_id()); + out_msg->set_height(1); + out_msg->set_measurement_time(scan_msg->basetime() / 1000000000.0); + out_msg->mutable_header()->set_sequence_num( + scan_msg->header().sequence_num()); + gps_base_usec_ = scan_msg->basetime(); + + frame_count++; + const unsigned char *difop_ptr = + (const unsigned char *)scan_msg->difop_pkts(0).data().c_str(); + + if (difop_ptr[0] == 0x00 || difop_ptr[0] == 0xa5) { + if (difop_ptr[1] == 0xff && difop_ptr[2] == 0x00 && difop_ptr[3] == 0x5a) { + if (difop_ptr[231] == 64 || difop_ptr[231] == 65) { + is_add_frame_ = true; + } + + int majorVersion = difop_ptr[1202]; + int minorVersion1 = difop_ptr[1203] / 16; + + // v1.1 :0.01 //v1.2以后 ： 0.0025 + if (1 > majorVersion || (1 == majorVersion && minorVersion1 > 1)) { + g_fAngleAcc_V = 0.0025; + } else { + g_fAngleAcc_V = 0.01; + } + + float fInitAngle_V = difop_ptr[188] * 256 + difop_ptr[189]; + if (fInitAngle_V > 32767) { + fInitAngle_V = fInitAngle_V - 65536; + } + this->prism_angle[0] = fInitAngle_V * g_fAngleAcc_V; + + fInitAngle_V = difop_ptr[190] * 256 + difop_ptr[191]; + if (fInitAngle_V > 32767) { + fInitAngle_V = fInitAngle_V - 65536; + } + this->prism_angle[1] = fInitAngle_V * g_fAngleAcc_V; + + fInitAngle_V = difop_ptr[192] * 256 + difop_ptr[193]; + if (fInitAngle_V > 32767) { + fInitAngle_V = fInitAngle_V - 65536; + } + this->prism_angle[2] = fInitAngle_V * g_fAngleAcc_V; + + fInitAngle_V = difop_ptr[194] * 256 + difop_ptr[195]; + if (fInitAngle_V > 32767) { + fInitAngle_V = fInitAngle_V - 65536; + } + this->prism_angle[3] = fInitAngle_V * g_fAngleAcc_V; + } + } + + packets_size = scan_msg->firing_pkts_size(); + packet_number_ = packets_size; + + for (size_t i = 0; i < packets_size; ++i) { + Unpack(static_cast(i), scan_msg->firing_pkts(static_cast(i)), + out_msg); + } + + if (is_add_frame_) { + if (frame_count >= 2) { + // out_msg = std::move(cur_pc); + for (int j = 0; j < cur_pc->point_size(); ++j) { + PointXYZIT *point3 = out_msg->add_point(); + point3->set_timestamp(cur_pc->point(j).timestamp()); + point3->set_intensity(cur_pc->point(j).intensity()); + point3->set_x(cur_pc->point(j).x()); + point3->set_y(cur_pc->point(j).y()); + point3->set_z(cur_pc->point(j).z()); + } + } + cur_pc = pre_pc; + pre_pc.reset(new PointCloud()); + } else { + // out_msg = cur_pc; + for (int j = 0; j < cur_pc->point_size(); ++j) { + PointXYZIT *point3 = out_msg->add_point(); + point3->set_timestamp(cur_pc->point(j).timestamp()); + point3->set_intensity(cur_pc->point(j).intensity()); + point3->set_x(cur_pc->point(j).x()); + point3->set_y(cur_pc->point(j).y()); + point3->set_z(cur_pc->point(j).z()); + } + cur_pc.reset(new PointCloud()); + pre_pc.reset(new PointCloud()); + } + AINFO << "line: " << __LINE__ << "out_msg size: " << out_msg->point_size(); + AINFO << "packets_size :" << packets_size; + if (out_msg->point().empty()) { + // we discard this pointcloud if empty + AERROR << "All points is NAN!Please check lslidar:" << config_.model(); + } + + // set default width + out_msg->set_width(out_msg->point_size()); + out_msg->set_height(1); +} + +/** @brief convert raw packet to point cloud + * @param pkt raw packet to Unpack + * @param pc shared pointer to point cloud (points are appended) + */ +void LslidarLS128S2Parser::Unpack(int num, const LslidarPacket &pkt, + std::shared_ptr out_msg) { + struct Firing_LS128S2 lidardata {}; + uint64_t packet_end_time; + const unsigned char *msop_ptr = (const unsigned char *)pkt.data().c_str(); + + packet_end_time = pkt.stamp(); + current_packet_time = packet_end_time; + if (msop_ptr[1205] == 0x02) { + return_mode = 2; + } + + if (return_mode == 1) { + double packet_interval_time = (current_packet_time - last_packet_time) / + (POINTS_PER_PACKET_SINGLE_ECHO / 8.0); + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET_SINGLE_ECHO; + point_idx += 8) { + if ((msop_ptr[point_idx] == 0xff) && (msop_ptr[point_idx + 1] == 0xaa) && + (msop_ptr[point_idx + 2] == 0xbb) && + (msop_ptr[point_idx + 3] == 0xcc) && + (msop_ptr[point_idx + 4] == 0xdd)) { + continue; + } else { + // Compute the time of the point + double point_time; + if (last_packet_time > 1e-6) { + point_time = + packet_end_time - + packet_interval_time * + ((POINTS_PER_PACKET_SINGLE_ECHO - point_idx) / 8 - 1); + } else { + point_time = current_packet_time; + } + + memset(&lidardata, 0, sizeof(lidardata)); + // 水平角度 + double fAngle_H = msop_ptr[point_idx + 1] + (msop_ptr[point_idx] << 8); + if (fAngle_H > 32767) { + fAngle_H = (fAngle_H - 65536); + } + lidardata.azimuth = fAngle_H * 0.01; + // 垂直角度+通道号 + int iTempAngle = msop_ptr[point_idx + 2]; + int iChannelNumber = iTempAngle >> 6; // 左移六位 通道号 + int iSymmbol = (iTempAngle >> 5) & 0x01; // 左移五位 符号位 + double fAngle_V = 0.0; + if (1 == iSymmbol) { // 符号位 0：正数 1：负数 + int iAngle_V = + msop_ptr[point_idx + 3] + (msop_ptr[point_idx + 2] << 8); + + fAngle_V = iAngle_V | 0xc000; + if (fAngle_V > 32767) { + fAngle_V = (fAngle_V - 65536); + } + } else { + int iAngle_Hight = iTempAngle & 0x3f; + fAngle_V = msop_ptr[point_idx + 3] + (iAngle_Hight << 8); + } + + lidardata.vertical_angle = fAngle_V * g_fAngleAcc_V; + lidardata.channel_number = iChannelNumber; + lidardata.distance = + ((msop_ptr[point_idx + 4] << 16) + (msop_ptr[point_idx + 5] << 8) + + msop_ptr[point_idx + 6]); + lidardata.intensity = msop_ptr[point_idx + 7]; + lidardata.time = point_time; + lidardata.azimuth = fAngle_H * 0.01; + convertCoordinate(lidardata); + } + } + } else { + double packet_interval_time = (current_packet_time - last_packet_time) / + (POINTS_PER_PACKET_DOUBLE_ECHO / 12.0); + for (size_t point_idx = 0; point_idx < POINTS_PER_PACKET_DOUBLE_ECHO; + point_idx += 12) { + if ((msop_ptr[point_idx] == 0xff) && (msop_ptr[point_idx + 1] == 0xaa) && + (msop_ptr[point_idx + 2] == 0xbb) && + (msop_ptr[point_idx + 3] == 0xcc) && + (msop_ptr[point_idx + 4] == 0xdd)) { + continue; + } else { + // Compute the time of the point + double point_time; + if (last_packet_time > 1e-6) { + point_time = + packet_end_time - + packet_interval_time * + ((POINTS_PER_PACKET_DOUBLE_ECHO - point_idx) / 12 - 1); + } else { + point_time = current_packet_time; + } + memset(&lidardata, 0, sizeof(lidardata)); + // 水平角度 + double fAngle_H = msop_ptr[point_idx + 1] + (msop_ptr[point_idx] << 8); + if (fAngle_H > 32767) { + fAngle_H = (fAngle_H - 65536); + } + lidardata.azimuth = fAngle_H * 0.01; + + // 垂直角度+通道号 + int iTempAngle = msop_ptr[point_idx + 2]; + int iChannelNumber = iTempAngle >> 6; // 左移六位 通道号 + int iSymmbol = (iTempAngle >> 5) & 0x01; // 左移五位 符号位 + double fAngle_V = 0.0; + if (1 == iSymmbol) { // 符号位 0：正数 1：负数 + int iAngle_V = + msop_ptr[point_idx + 3] + (msop_ptr[point_idx + 2] << 8); + + fAngle_V = iAngle_V | 0xc000; + if (fAngle_V > 32767) { + fAngle_V = (fAngle_V - 65536); + } + } else { + int iAngle_Hight = iTempAngle & 0x3f; + fAngle_V = msop_ptr[point_idx + 3] + (iAngle_Hight << 8); + } + + lidardata.vertical_angle = fAngle_V * g_fAngleAcc_V; + lidardata.channel_number = iChannelNumber; + lidardata.distance = + ((msop_ptr[point_idx + 4] << 16) + (msop_ptr[point_idx + 5] << 8) + + msop_ptr[point_idx + 6]); + lidardata.intensity = msop_ptr[point_idx + 7]; + lidardata.time = point_time; + convertCoordinate(lidardata); // 第一个点 + + lidardata.distance = + ((msop_ptr[point_idx + 8] << 16) + (msop_ptr[point_idx + 9] << 8) + + msop_ptr[point_idx + 10]); + lidardata.intensity = msop_ptr[point_idx + 11]; + lidardata.time = point_time; + convertCoordinate(lidardata); // 第二个点 + } + } + last_packet_time = packet_end_time; + } +} + +int LslidarLS128S2Parser::convertCoordinate( + const struct Firing_LS128S2 &lidardata) { + if (lidardata.distance * g_fDistanceAcc > config_.max_range() || + lidardata.distance * g_fDistanceAcc < config_.min_range()) { + return -1; + } + + if ((lidardata.azimuth < config_.scan_start_angle()) || + (lidardata.azimuth > config_.scan_end_angle())) { + return -1; + } + + double fAngle_H = 0.0; // 水平角度 + double fAngle_V = 0.0; // 垂直角度 + fAngle_H = lidardata.azimuth; + fAngle_V = lidardata.vertical_angle; + + // 加畸变 + double fSinV_angle = 0; + double fCosV_angle = 0; + + // 振镜偏移角度 = 实际垂直角度 / 2 - 偏移值 + double fGalvanometrtAngle = 0; + fGalvanometrtAngle = fAngle_V + 7.26; + + while (fGalvanometrtAngle < 0.0) { + fGalvanometrtAngle += 360.0; + } + while (fAngle_H < 0.0) { + fAngle_H += 360.0; + } + + int table_index_V = static_cast(fGalvanometrtAngle * 100) % 36000; + int table_index_H = static_cast(fAngle_H * 100) % 36000; + + double fAngle_R0 = + cos30 * cos_mirror_angle[lidardata.channel_number % 4] * + cos_table[table_index_V] - + sin_table[table_index_V] * sin_mirror_angle[lidardata.channel_number % 4]; + + fSinV_angle = 2 * fAngle_R0 * sin_table[table_index_V] + + sin_mirror_angle[lidardata.channel_number % 4]; + fCosV_angle = sqrt(1 - pow(fSinV_angle, 2)); + + double fSinCite = (2 * fAngle_R0 * cos_table[table_index_V] * sin30 - + cos_mirror_angle[lidardata.channel_number % 4] * sin60) / + fCosV_angle; + double fCosCite = sqrt(1 - pow(fSinCite, 2)); + + double fSinCite_H = + sin_table[table_index_H] * fCosCite + cos_table[table_index_H] * fSinCite; + double fCosCite_H = + cos_table[table_index_H] * fCosCite - sin_table[table_index_H] * fSinCite; + + double x_coord = 0.0, y_coord = 0.0, z_coord = 0.0; + x_coord = (lidardata.distance * fCosV_angle * fSinCite_H) * g_fDistanceAcc; + y_coord = (lidardata.distance * fCosV_angle * fCosCite_H) * g_fDistanceAcc; + z_coord = (lidardata.distance * fSinV_angle) * g_fDistanceAcc; + + if ((y_coord >= config_.bottom_left_x() && + y_coord <= config_.top_right_x()) && + (-x_coord >= config_.bottom_left_y() && + -x_coord <= config_.top_right_y())) + return -1; + + PointXYZIT *point1 = cur_pc->add_point(); + point1->set_timestamp(lidardata.time); + point1->set_intensity(lidardata.intensity); + point1->set_x(y_coord); + point1->set_y(-x_coord); + point1->set_z(z_coord); + + PointXYZIT *point2 = pre_pc->add_point(); + point2->set_timestamp(lidardata.time); + point2->set_intensity(lidardata.intensity); + point2->set_x(y_coord); + point2->set_y(-x_coord); + point2->set_z(z_coord); + return 0; +} + +int LslidarLS128S2Parser::convertCoordinate( + const struct Firing_LS128S2 &lidardata, + std::shared_ptr out_cloud) { + if (lidardata.distance * g_fDistanceAcc > config_.max_range() || + lidardata.distance * g_fDistanceAcc < config_.min_range()) { + return -1; + } + + if ((lidardata.azimuth < config_.scan_start_angle()) || + (lidardata.azimuth > config_.scan_end_angle())) { + return -1; + } + + double fAngle_H = 0.0; // 水平角度 + double fAngle_V = 0.0; // 垂直角度 + fAngle_H = lidardata.azimuth; + fAngle_V = lidardata.vertical_angle; + + // 加畸变 + double fSinV_angle = 0; + double fCosV_angle = 0; + + // 振镜偏移角度 = 实际垂直角度 / 2 - 偏移值 + double fGalvanometrtAngle = 0; + fGalvanometrtAngle = fAngle_V + 7.26; + + while (fGalvanometrtAngle < 0.0) { + fGalvanometrtAngle += 360.0; + } + while (fAngle_H < 0.0) { + fAngle_H += 360.0; + } + + int table_index_V = static_cast(fGalvanometrtAngle * 100) % 36000; + int table_index_H = static_cast(fAngle_H * 100) % 36000; + + double fAngle_R0 = + cos30 * cos_mirror_angle[lidardata.channel_number % 4] * + cos_table[table_index_V] - + sin_table[table_index_V] * sin_mirror_angle[lidardata.channel_number % 4]; + + fSinV_angle = 2 * fAngle_R0 * sin_table[table_index_V] + + sin_mirror_angle[lidardata.channel_number % 4]; + fCosV_angle = sqrt(1 - pow(fSinV_angle, 2)); + + double fSinCite = (2 * fAngle_R0 * cos_table[table_index_V] * sin30 - + cos_mirror_angle[lidardata.channel_number % 4] * sin60) / + fCosV_angle; + double fCosCite = sqrt(1 - pow(fSinCite, 2)); + + double fSinCite_H = + sin_table[table_index_H] * fCosCite + cos_table[table_index_H] * fSinCite; + double fCosCite_H = + cos_table[table_index_H] * fCosCite - sin_table[table_index_H] * fSinCite; + + double x_coord = 0.0, y_coord = 0.0, z_coord = 0.0; + x_coord = (lidardata.distance * fCosV_angle * fSinCite_H) * g_fDistanceAcc; + y_coord = (lidardata.distance * fCosV_angle * fCosCite_H) * g_fDistanceAcc; + z_coord = (lidardata.distance * fSinV_angle) * g_fDistanceAcc; + + if ((y_coord >= config_.bottom_left_x() && + y_coord <= config_.top_right_x()) && + (-x_coord >= config_.bottom_left_y() && + -x_coord <= config_.top_right_y())) + return -1; + + PointXYZIT *point1 = cur_pc->add_point(); + point1->set_timestamp(lidardata.time); + point1->set_intensity(lidardata.intensity); + point1->set_x(y_coord); + point1->set_y(-x_coord); + point1->set_z(z_coord); + + PointXYZIT *point2 = pre_pc->add_point(); + point2->set_timestamp(lidardata.time); + point2->set_intensity(lidardata.intensity); + point2->set_x(y_coord); + point2->set_y(-x_coord); + point2->set_z(z_coord); + + PointXYZIT *point3 = out_cloud->add_point(); + point3->set_timestamp(lidardata.time); + point3->set_intensity(lidardata.intensity); + point3->set_x(y_coord); + point3->set_y(-x_coord); + point3->set_z(z_coord); + return 0; +} + +void LslidarLS128S2Parser::Order(std::shared_ptr cloud) {} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidar_convert_component.cc b/modules/drivers/lidar/lslidar/parser/lslidar_convert_component.cc new file mode 100644 index 00000000000..fdcd57ab7de --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidar_convert_component.cc @@ -0,0 +1,88 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/parser/lslidar_convert_component.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +static void my_handler(int sig) { exit(-1); } + +bool LslidarConvertComponent::Init() { + signal(SIGINT, my_handler); + Config lslidar_config; + if (!GetProtoConfig(&lslidar_config)) { + AWARN << "Load config failed, config file" << config_file_path_; + return false; + } + + conv_.reset(new Convert()); + conv_->init(lslidar_config); + + writer_ = node_->CreateWriter( + lslidar_config.convert_channel_name()); + point_cloud_pool_.reset( + new CCObjectPool(pool_size_)); + point_cloud_pool_->ConstructAll(); + + for (int i = 0; i < pool_size_; i++) { + auto point_cloud = point_cloud_pool_->GetObject(); + if (point_cloud == nullptr) { + AERROR << "fail to getobject, i: " << i; + return false; + } + point_cloud->mutable_point()->Reserve(200000); + } + + AINFO << "Point cloud comp convert init success"; + return true; +} + +bool LslidarConvertComponent::Proc( + const std::shared_ptr& scan_msg) { + std::shared_ptr point_cloud_out = + point_cloud_pool_->GetObject(); + if (point_cloud_out == nullptr) { + AWARN << "poin cloud pool return nullptr, will be create new."; + point_cloud_out = std::make_shared(); + point_cloud_out->mutable_point()->Reserve(200000); + } + if (point_cloud_out == nullptr) { + AWARN << "point cloud out is nullptr"; + return false; + } + + uint64_t time1 = apollo::cyber::Time().Now().ToNanosecond(); + AINFO << "receive scan!---------"; + point_cloud_out->Clear(); + conv_->ConvertPacketsToPointcloud(scan_msg, point_cloud_out); + uint64_t time2 = apollo::cyber::Time().Now().ToNanosecond(); + AINFO << "process pointcloud time: " << (time2 - time1) / 1000000000.0; + + if (point_cloud_out == nullptr || point_cloud_out->point().empty()) { + AWARN << "point_cloud_out convert is empty."; + return false; + } + + writer_->Write(point_cloud_out); + + return true; +} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidar_convert_component.h b/modules/drivers/lidar/lslidar/parser/lslidar_convert_component.h new file mode 100644 index 00000000000..10d90939948 --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidar_convert_component.h @@ -0,0 +1,60 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#pragma once + +#include +#include +#include + +#include "modules/drivers/lidar/lslidar/proto/config.pb.h" +#include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" + +#include "cyber/base/concurrent_object_pool.h" +#include "cyber/cyber.h" +#include "modules/drivers/lidar/lslidar/parser/convert.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +using apollo::cyber::Component; +using apollo::cyber::Reader; +using apollo::cyber::Writer; +using apollo::cyber::base::CCObjectPool; +using apollo::drivers::PointCloud; +using apollo::drivers::lslidar::LslidarScan; + +class LslidarConvertComponent + : public Component { + public: + bool Init() override; + bool Proc(const std::shared_ptr& + scan_msg) override; + + private: + std::shared_ptr> writer_; + std::unique_ptr conv_ = nullptr; + std::shared_ptr> point_cloud_pool_ = + nullptr; + int pool_size_ = 8; +}; + +CYBER_REGISTER_COMPONENT(LslidarConvertComponent) + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidar_parser.cc b/modules/drivers/lidar/lslidar/parser/lslidar_parser.cc new file mode 100644 index 00000000000..5d45f2119b9 --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidar_parser.cc @@ -0,0 +1,306 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar/lslidar/parser/lslidar_parser.h" + +namespace apollo { +namespace drivers { +namespace lslidar { + +LslidarParser::LslidarParser(const Config &config) + : last_time_stamp_(0), config_(config) { + for (int i = 0; i < 4; ++i) { + prism_angle64[i] = i * 0.35; + } + for (int j = 0; j < 128; ++j) { + // 右边 + if (j / 4 % 2 == 0) { + theat1_s[j] = sin((-25 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180); + theat1_c[j] = cos((-25 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180); + } else { // 左边 + theat1_s[j] = sin((-24 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_s[j] = sin(prism_angle64[j % 4] * M_PI / 180); + theat1_c[j] = cos((-24 + floor(j / 8) * 2.5) * M_PI / 180); + theat2_c[j] = cos(prism_angle64[j % 4] * M_PI / 180); + } + } +} + +bool LslidarParser::is_scan_valid(int rotation, float range) { + // check range first + if (range < config_.min_range() || range > config_.max_range()) + return false; + else + return true; +} + +/** Set up for on-line operation. */ +void LslidarParser::setup() { + if (config_.calibration()) { + calibration_.read(config_.calibration_file()); + + if (!calibration_.initialized_) { + AFATAL << "Unable to open calibration file: " + << config_.calibration_file(); + } + } + for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { + float rotation_f = + ROTATION_RESOLUTION * static_cast(rot_index * M_PI) / 180.0f; + cos_azimuth_table[rot_index] = cosf(rotation_f); + sin_azimuth_table[rot_index] = sinf(rotation_f); + } +} + +void LslidarParser::ComputeCoords(const float &raw_distance, + LaserCorrection *corrections, + const uint16_t rotation, PointXYZIT *point) { + double x = 0.0; + double y = 0.0; + double z = 0.0; + double distance_corr_x = 0; + double distance_corr_y = 0; + + corrections->cos_rot_correction = cosf(corrections->rot_correction); + corrections->sin_rot_correction = sinf(corrections->rot_correction); + corrections->cos_vert_correction = cosf(corrections->vert_correction); + corrections->sin_vert_correction = sinf(corrections->vert_correction); + + double distance = raw_distance + corrections->dist_correction; + + double cos_rot_angle = + cos_azimuth_table[rotation] * corrections->cos_rot_correction + + sin_azimuth_table[rotation] * corrections->sin_rot_correction; + double sin_rot_angle = + sin_azimuth_table[rotation] * corrections->cos_rot_correction - + cos_azimuth_table[rotation] * corrections->sin_rot_correction; + + // Compute the distance in the xy plane (w/o accounting for rotation) + double xy_distance = distance * corrections->cos_vert_correction; + + // Get 2points calibration values,Linear interpolation to get distance + // correction for X and Y, that means distance correction use + // different value at different distance + distance_corr_x = distance_corr_y = corrections->dist_correction; + + double distance_x = raw_distance + distance_corr_x; + xy_distance = distance_x * corrections->cos_vert_correction; + + x = xy_distance * sin_rot_angle - + corrections->horiz_offset_correction * cos_rot_angle; + + double distance_y = raw_distance + distance_corr_y; + xy_distance = distance_y * corrections->cos_vert_correction; + + y = xy_distance * cos_rot_angle + + corrections->horiz_offset_correction * sin_rot_angle; + z = distance * corrections->sin_vert_correction + + corrections->vert_offset_correction; + + /** Use standard ROS coordinate system (right-hand rule) */ + point->set_x(static_cast(-y)); + point->set_y(static_cast(-x)); + point->set_z(static_cast(z)); +} + +void LslidarParser::ComputeCoords2(int Laser_ring, int Type, + const float &raw_distance, + LaserCorrection* corrections, + const double rotation, PointXYZIT *point) { + double x = 0.0; + double y = 0.0; + double z = 0.0; + double distance_corr_x = 0; + double distance_corr_y = 0; + double sinTheta_1[128] = {0}; + double cosTheta_1[128] = {0}; + double sinTheta_2[128] = {0}; + double cosTheta_2[128] = {0}; + double cos_xita; + // 垂直角度 + double sin_theat; + double cos_theat; + double _R_; + // 水平角度 + double cos_H_xita; + double sin_H_xita; + double cos_xita_F; + + for (int i = 0; i < 128; i++) { + sinTheta_1[i] = sin(big_angle[i / 4] * M_PI / 180); + cosTheta_1[i] = cos(big_angle[i / 4] * M_PI / 180); + + if (abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6 && + abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6) { + sinTheta_2[i] = sin((i % 4) * (-0.17) * M_PI / 180); + cosTheta_2[i] = cos((i % 4) * (-0.17) * M_PI / 180); + } else { + sinTheta_2[i] = sin(prism_angle[i % 4] * M_PI / 180); + cosTheta_2[i] = cos(prism_angle[i % 4] * M_PI / 180); + } + } + + corrections->cos_rot_correction = cosf(corrections->rot_correction); + corrections->sin_rot_correction = sinf(corrections->rot_correction); + + double cos_azimuth_half = cos(rotation * 0.5); + + if (Type == CH16) { + if (abs(prism_angle[0]) < 1e-6 && abs(prism_angle[1]) < 1e-6 && + abs(prism_angle[2]) < 1e-6 && abs(prism_angle[3]) < 1e-6) { + corrections->sin_vert_correction = + sin_scan_laser_altitude[Laser_ring / 4 + 2] + + 2 * cos_azimuth_half * sin_scan_mirror_altitude[Laser_ring % 4]; + } else { + corrections->sin_vert_correction = + sin_scan_laser_altitude[Laser_ring / 4 + 2] + + 2 * cos_azimuth_half * sin(prism_angle[Laser_ring % 4] * M_PI / 180); + } + } else if (Type == CH32) { + corrections->sin_vert_correction = + sin_scan_laser_altitude[Laser_ring / 4] + + 2 * cos_azimuth_half * sin_scan_mirror_altitude[Laser_ring % 4]; + } else if (Type == CH64) { + if (Laser_ring % 8 == 0 || Laser_ring % 8 == 1 || Laser_ring % 8 == 2 || + Laser_ring % 8 == 3) { + corrections->sin_vert_correction = + sin(-13.33 * DEG_TO_RAD + floor(Laser_ring / 4) * 1.33 * DEG_TO_RAD) + + 2 * cos(rotation / 2 + 1.05 * DEG_TO_RAD) * + sin((Laser_ring % 4) * 0.33 * DEG_TO_RAD); + } else if (Laser_ring % 8 == 4 || Laser_ring % 8 == 5 || + Laser_ring % 8 == 6 || Laser_ring % 8 == 7) { + corrections->sin_vert_correction = + sin(-13.33 * DEG_TO_RAD + floor(Laser_ring / 4) * 1.33 * DEG_TO_RAD) + + 2 * cos(rotation / 2 - 1.05 * DEG_TO_RAD) * + sin((Laser_ring % 4) * 0.33 * DEG_TO_RAD); + } + } else if (Type == CH64w) { + if (Laser_ring / 4 % 2 == 0) { + cos_xita = cos((rotation / 2.0 + 22.5) * M_PI / 180); + } else { + cos_xita = cos((-rotation / 2.0 + 112.5) * M_PI / 180); + } + _R_ = theat2_c[Laser_ring] * theat1_c[Laser_ring] * cos_xita - + theat2_s[Laser_ring] * theat1_s[Laser_ring]; + sin_theat = theat1_s[Laser_ring] + 2 * _R_ * theat2_s[Laser_ring]; + cos_theat = sqrt(1 - pow(sin_theat, 2)); + cos_H_xita = + (2 * _R_ * theat2_c[Laser_ring] * cos_xita - theat1_c[Laser_ring]) / + cos_theat; + sin_H_xita = sqrt(1 - pow(cos_H_xita, 2)); + + if (Laser_ring / 4 % 2 == 0) { + cos_xita_F = (cos_H_xita + sin_H_xita) * sqrt(0.5); + corrections->sin_vert_correction = sqrt(1 - pow(cos_xita_F, 2)); + } else { + cos_xita_F = (cos_H_xita + sin_H_xita) * (-sqrt(0.5)); + corrections->sin_vert_correction = sqrt(1 - pow(cos_xita_F, 2)); + } + } else if (Type == CH120) { + corrections->sin_vert_correction = sinf(corrections->vert_correction); + } else if (Type == CH128) { + if (Laser_ring / 4 % 2 == 0) { + cos_azimuth_half = sin((rotation - 15 * DEG_TO_RAD) * 0.5); + } else { + cos_azimuth_half = cos((rotation + 15 * DEG_TO_RAD) * 0.5); + } + corrections->sin_vert_correction = + sin_scan_laser_altitude_ch128[Laser_ring / 4] + + 2 * cos_azimuth_half * sinTheta_2[Laser_ring]; + } else if (Type == CH128X1) { + double _R_ = + cosTheta_2[Laser_ring] * cosTheta_1[Laser_ring] * cos_azimuth_half - + sinTheta_2[Laser_ring] * sinTheta_1[Laser_ring]; + corrections->sin_vert_correction = + sinTheta_1[Laser_ring] + 2 * _R_ * sinTheta_2[Laser_ring]; + } + corrections->cos_vert_correction = + sqrt(1 - pow(corrections->sin_vert_correction, 2)); + + double distance = raw_distance + corrections->dist_correction; + + double cos_rot_angle = cos(rotation) * corrections->cos_rot_correction + + sin(rotation) * corrections->sin_rot_correction; + double sin_rot_angle = sin(rotation) * corrections->cos_rot_correction - + cos(rotation) * corrections->sin_rot_correction; + + // Compute the distance in the xy plane (w/o accounting for rotation) + double xy_distance = distance * corrections->cos_vert_correction; + + // Get 2points calibration values,Linear interpolation to get distance + // correction for X and Y, that means distance correction use + // different value at different distance + distance_corr_x = distance_corr_y = corrections->dist_correction; + + double distance_x = raw_distance + distance_corr_x; + xy_distance = distance_x * corrections->cos_vert_correction; + + x = xy_distance * sin_rot_angle - + corrections->horiz_offset_correction * cos_rot_angle; + + double distance_y = raw_distance + distance_corr_y; + xy_distance = distance_y * corrections->cos_vert_correction; + + y = xy_distance * cos_rot_angle + + corrections->horiz_offset_correction * sin_rot_angle; + z = distance * corrections->sin_vert_correction + + corrections->vert_offset_correction; + + /** Use standard ROS coordinate system (right-hand rule) */ + point->set_x(static_cast(-y)); + point->set_y(static_cast(-x)); + point->set_z(static_cast(z)); +} + +LslidarParser *LslidarParserFactory::CreateParser(Config source_config) { + Config config = source_config; + + if (config.model() == LSLIDAR16P) { + return new Lslidar16Parser(config); + } else if (config.model() == LSLIDAR32P) { + return new Lslidar32Parser(config); + } else if (config.model() == LSLIDAR_C32_V4 || + config.model() == LSLIDAR_C16_V4 || + config.model() == LSLIDAR_C8_V4 || + config.model() == LSLIDAR_C1_V4) { + return new LslidarCXV4Parser(config); + } else if (config.model() == LSLIDAR_CH16) { + return new LslidarCH16Parser(config); + } else if (config.model() == LSLIDAR_CH32) { + return new LslidarCH32Parser(config); + } else if (config.model() == LSLIDAR_CH64) { + return new LslidarCH64Parser(config); + } else if (config.model() == LSLIDAR_CH64w) { + return new LslidarCH64wParser(config); + } else if (config.model() == LSLIDAR_CH120) { + return new LslidarCH120Parser(config); + } else if (config.model() == LSLIDAR_CH128) { + return new LslidarCH128Parser(config); + } else if (config.model() == LSLIDAR_CH128X1) { + return new LslidarCH128X1Parser(config); + } else if (config.model() == LSLIDAR_LS128S2) { + return new LslidarLS128S2Parser(config); + } else { + AERROR << "invalid model"; + return nullptr; + } +} + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/parser/lslidar_parser.h b/modules/drivers/lidar/lslidar/parser/lslidar_parser.h new file mode 100644 index 00000000000..50e5c90200d --- /dev/null +++ b/modules/drivers/lidar/lslidar/parser/lslidar_parser.h @@ -0,0 +1,866 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +/* -*- mode: C++ -*- + * + * Copyright (C) 2007 Austin Robot Technology, Yaxin Liu, Patrick Beeson + * Copyright (C) 2009 Austin Robot Technology, Jack O'Quin + * + * License: Modified BSD Software License Agreement + * + * $Id$ + */ + +/** \file + * \ingroup lslidar + * + * These classes Unpack raw Lslidar LIDAR packets into several + * useful formats. + * + * lslidar::Data -- virtual base class for unpacking data into + * various formats + * + * lslidar::DataScans -- derived class, unpacks into vector of + * individual laser scans + * + * lslidar::DataXYZ -- derived class, unpacks into XYZ format + * + * \todo make a separate header for each class? + * + * \author Yaxin Liu + * \author Patrick Beeson + * \author Jack O'Quin + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h" +#include "modules/drivers/lidar/lslidar/proto/config.pb.h" +#include "modules/drivers/lidar/lslidar/proto/lslidar.pb.h" + +#include "cyber/cyber.h" +#include "modules/drivers/lidar/lslidar/parser/calibration.h" + +#define DEG_TO_RAD 0.017453292 +#define RAD_TO_DEG 57.29577951 + +#define CH16 1 +#define CH32 2 +#define CH64 3 +#define CH64w 4 +#define CH120 5 +#define CH128 6 +#define CH128X1 7 + +namespace apollo { +namespace drivers { +namespace lslidar { + +using apollo::drivers::PointCloud; +using apollo::drivers::PointXYZIT; +using apollo::drivers::lslidar::LSLIDAR16P; +using apollo::drivers::lslidar::LslidarPacket; +using apollo::drivers::lslidar::LslidarScan; + +/** + * Raw Lslidar packet constants and structures. + */ +#define DEG2RAD(x) ((x)*0.017453293) +static const int SIZE_BLOCK = 100; +static const int RAW_SCAN_SIZE = 3; +static const int SCANS_PER_BLOCK = 32; +static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE); + +static const float ROTATION_RESOLUTION = 0.01f; /**< degrees */ +static const uint16_t ROTATION_MAX_UNITS = 36000; + +static const unsigned int POINTS_ONE_CHANNEL_PER_SECOND = 20000; +static const unsigned int BLOCKS_ONE_CHANNEL_PER_PKT = 12; + +/** According to Bruce Hall DISTANCE_MAX is 65.0, but we noticed + * valid packets with readings up to 200.0. */ +static const float DISTANCE_MAX = 200.0f; /**< meters */ +static const float DISTANCE_MIN = 0.2f; +static const float DISTANCE_RESOLUTION = 0.01f; /**< meters */ +static const double DISTANCE_RESOLUTION2 = 0.0000390625; +static const float DISTANCE_RESOLUTION_NEW = 0.005f; +static const float DISTANCE_MAX_UNITS = + (DISTANCE_MAX / DISTANCE_RESOLUTION + 1.0f); + +// laser_block_id +static const uint16_t UPPER_BANK = 0xeeff; +static const uint16_t LOWER_BANK = 0xddff; + +/** Special Defines for VLP16 support **/ +static const int LSC16_FIRINGS_PER_BLOCK = 2; +static const int LSC16_SCANS_PER_FIRING = 16; +static const float LSC16_BLOCK_TDURATION = 100.0f; // [µs] +static const float LSC16_DSR_TOFFSET = 3.125f; // [µs] +static const float LSC16_FIRING_TOFFSET = 50.0f; // [µs] + +static const int LSC32_FIRINGS_PER_BLOCK = 1; +static const int LSC32_SCANS_PER_FIRING = 32; +static const float LSC32_FIRING_TOFFSET = 100.0f; // [µs] +static const float LSC32_AZIMUTH_TOFFSET = 12.98f; /**< meters */ +static const float LSC32_DISTANCE_TOFFSET = 4.94f; /**< meters */ + +static const int SCANS_PER_FIRING_C32 = 32; +static const int FIRING_TOFFSET_C32 = 32; +static const double R32_1_ = 0.0361; +static const double CX4_R1_ = 0.0361; // +static const double CX4_C32W = 0.03416; // 新C32W byte[1211] = 0x47 +static const double CX4_R1_90 = 0.0209; // 90度 +static const double CX4_conversionAngle_ = 20.25; +static const double CX4_conversionAngle_90 = 27.76; // 90度 + +/** \brief Raw Lslidar data block. + * + * Each block contains data from either the upper or lower laser + * bank. The device returns three times as many upper bank blocks. + * + * use cstdint types, so things work with both 64 and 32-bit machines + */ +typedef struct raw_block { + uint16_t header; ///< UPPER_BANK or LOWER_BANK + uint8_t rotation_1; + uint8_t rotation_2; /// combine rotation1 and rotation2 together to get + /// 0-35999, divide by 100 to get degrees + uint8_t data[BLOCK_DATA_SIZE]; // 96 +} raw_block_t; + +/** used for unpacking the first two data bytes in a block + * + * They are packed into the actual data stream misaligned. I doubt + * this works on big endian machines. + */ +union two_bytes { + int16_t uint; + int8_t bytes[2]; +}; + +union four_bytes { + uint32_t uint; + uint8_t bytes[4]; +}; +static const int PACKET_SIZE = 1212; +static const int POINTS_PER_PACKET = 171; +static const int LS_POINTS_PER_PACKET = 149; // LS128S2 149 points +static const int BLOCKS_PER_PACKET = 12; +static const int PACKET_STATUS_SIZE = 4; +static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET); + +const int ORDER_16[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}; +const int ORDER_32[32] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, + 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, + 22, 23, 24, 25, 26, 27, 28, 29, 30, 31}; +// 1,v2.6 +static const double scan_altitude_original_A[32] = { + -16, -15, -14, -13, -12, -11, -10, -9, -8, -7, -6, -5, -4, -3, -2, -1, + 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15}; + +// 1,v3.0 +static const double scan_altitude_original_A3[32] = { + -16, 0, -15, 1, -14, 2, -13, 3, -12, 4, -11, 5, -10, 6, -9, 7, + -8, 8, -7, 9, -6, 10, -5, 11, -4, 12, -3, 13, -2, 14, -1, 15}; + +// 0.33,v2.6 +static const double scan_altitude_original_C[32] = { + -18, -15, -12, -10, -8, -7, -6, -5, -4, -3.33, -2.66, + -3, -2.33, -2, -1.33, -1.66, -1, -0.66, 0, -0.33, 0.33, 0.66, + 1.33, 1, 1.66, 2, 3, 4, 6, 8, 11, 14}; + +// 0.33,v3.0 +static const double scan_altitude_original_C3[32] = { + -18, -1, -15, -0.66, -12, -0.33, -10, 0, -8, 0.33, -7, + 0.66, -6, 1, -5, 1.33, -4, 1.66, -3.33, 2, -3, 3, + -2.66, 4, -2.33, 6, -2, 8, -1.66, 11, -1.33, 14}; + +// CX 4.0 +// Pre-compute the sine and cosine for the altitude angles. +// c32 32度 +static const double c32_32_vertical_angle[32] = { + -16, -8, 0, 8, -15, -7, 1, 9, -14, -6, 2, 10, -13, -5, 3, 11, + -12, -4, 4, 12, -11, -3, 5, 13, -10, -2, 6, 14, -9, -1, 7, 15}; +// c32 70度 0x46 +static const double c32_70_vertical_angle[32] = { + -54, -31, -8, 2.66, -51.5, -28, -6.66, 4, -49, -25, -5.33, + 5.5, -46, -22, -4, 7, -43, -18.5, -2.66, 9, -40, -15, + -1.33, 11, -37, -12, 0, 13, -34, -10, 1.33, 15}; + +// c32 70度 0x47 +static const double c32_70_vertical_angle2[32] = { + -54.7, -31, -9, 3, -51.5, -28, -7.5, 4.5, -49, -25, -6.0, + 6.0, -46, -22, -4.5, 7.5, -43, -18.5, -3.0, 9, -40, -15, + -1.5, 11, -37, -12, 0, 13, -34, -10.5, 1.5, 15}; + +// c32 90度 +static const double c32_90_vertical_angle[32] = { + 2.487, 25.174, 47.201, 68.819, 5.596, 27.811, 49.999, 71.525, + 8.591, 30.429, 52.798, 74.274, 11.494, 33.191, 55.596, 77.074, + 14.324, 36.008, 58.26, 79.938, 17.096, 38.808, 60.87, 82.884, + 19.824, 41.603, 63.498, 85.933, 22.513, 44.404, 66.144, 89.105}; + +static const double c16_vertical_angle[16] = { + -16, 0, -14, 2, -12, 4, -10, 6, -8, 8, -6, 10, -4, 12, -2, 14}; + +static const double c8_vertical_angle[8] = {-12, 4, -8, 8, -4, 10, 0, 12}; +static const double c1_vertical_angle[8] = {0, 0, 0, 0, 0, 0, 0, 0}; + +static const double scan_altitude_original_2[16] = { + -0.2617993877991494, 0.017453292519943295, -0.22689280275926285, + 0.05235987755982989, -0.19198621771937624, 0.08726646259971647, + -0.15707963267948966, 0.12217304763960307, -0.12217304763960307, + 0.15707963267948966, -0.08726646259971647, 0.19198621771937624, + -0.05235987755982989, 0.22689280275926285, -0.017453292519943295, + 0.2617993877991494}; +static const double scan_altitude_original_1[16] = { + -0.17453292519943295, 0.01160643952576229, -0.15123277968530863, + 0.03490658503988659, -0.12793263417118436, 0.05811946409141117, + -0.10471975511965978, 0.08141960960553547, -0.08141960960553547, + 0.10471975511965978, -0.05811946409141117, 0.12793263417118436, + -0.03490658503988659, 0.15123277968530863, -0.01160643952576229, + 0.17453292519943295}; + +static const double scan_laser_altitude[8] = { + -0.11641346110802178, -0.09302604913129776, -0.06981317007977318, + -0.04642575810304917, -0.023212879051524585, -0.0, + 0.023212879051524585, 0.04642575810304917, +}; + +static const double scan_laser_altitude_ch32[8] = { + -0.24434609527920614, -0.19198621771937624, -0.13962634015954636, + -0.08726646259971647, -0.03490658503988659, -0.0, + 0.03490658503988659, 0.08726646259971647, +}; + +static const double big_angle[32] = { + -17, -16, -15, -14, -13, -12, -11, -10, -9, -8, -7, + -6, -5, -4.125, -4, -3.125, -3, -2.125, -2, -1.125, -1, -0.125, + 0, 0.875, 1, 1.875, 2, 3, 4, 5, 6, 7}; + +static const double sin_scan_laser_altitude[8] = { + std::sin(scan_laser_altitude[0]), std::sin(scan_laser_altitude[1]), + std::sin(scan_laser_altitude[2]), std::sin(scan_laser_altitude[3]), + std::sin(scan_laser_altitude[4]), std::sin(scan_laser_altitude[5]), + std::sin(scan_laser_altitude[6]), std::sin(scan_laser_altitude[7]), +}; + +static const double sin_scan_laser_altitude_ch32[8] = { + std::sin(scan_laser_altitude_ch32[0]), + std::sin(scan_laser_altitude_ch32[1]), + std::sin(scan_laser_altitude_ch32[2]), + std::sin(scan_laser_altitude_ch32[3]), + std::sin(scan_laser_altitude_ch32[4]), + std::sin(scan_laser_altitude_ch32[5]), + std::sin(scan_laser_altitude_ch32[6]), + std::sin(scan_laser_altitude_ch32[7]), +}; + +// Pre-compute the sine and cosine for the altitude angles. +static const double scan_laser_altitude_ch128[32] = { + -0.29670597283903605, -0.2792526803190927, -0.2617993877991494, + -0.24434609527920614, -0.22689280275926285, -0.20943951023931956, + -0.19198621771937624, -0.17453292519943295, -0.15707963267948966, + -0.13962634015954636, -0.12217304763960307, -0.10471975511965978, + -0.08726646259971647, -0.06981317007977318, -0.05235987755982989, + -0.03490658503988659, -0.017453292519943295, 0.0, + 0.017453292519943295, 0.03490658503988659, 0.05235987755982989, + 0.06981317007977318, 0.08726646259971647, 0.10471975511965978, + 0.12217304763960307, 0.13962634015954636, 0.15707963267948966, + 0.17453292519943295, 0.19198621771937624, 0.20943951023931956, + 0.22689280275926285, 0.24434609527920614, +}; + +static const double sin_scan_laser_altitude_ch128[32] = { + std::sin(scan_laser_altitude_ch128[0]), + std::sin(scan_laser_altitude_ch128[1]), + std::sin(scan_laser_altitude_ch128[2]), + std::sin(scan_laser_altitude_ch128[3]), + std::sin(scan_laser_altitude_ch128[4]), + std::sin(scan_laser_altitude_ch128[5]), + std::sin(scan_laser_altitude_ch128[6]), + std::sin(scan_laser_altitude_ch128[7]), + std::sin(scan_laser_altitude_ch128[8]), + std::sin(scan_laser_altitude_ch128[9]), + std::sin(scan_laser_altitude_ch128[10]), + std::sin(scan_laser_altitude_ch128[11]), + std::sin(scan_laser_altitude_ch128[12]), + std::sin(scan_laser_altitude_ch128[13]), + std::sin(scan_laser_altitude_ch128[14]), + std::sin(scan_laser_altitude_ch128[15]), + std::sin(scan_laser_altitude_ch128[16]), + std::sin(scan_laser_altitude_ch128[17]), + std::sin(scan_laser_altitude_ch128[18]), + std::sin(scan_laser_altitude_ch128[19]), + std::sin(scan_laser_altitude_ch128[20]), + std::sin(scan_laser_altitude_ch128[21]), + std::sin(scan_laser_altitude_ch128[22]), + std::sin(scan_laser_altitude_ch128[23]), + std::sin(scan_laser_altitude_ch128[24]), + std::sin(scan_laser_altitude_ch128[25]), + std::sin(scan_laser_altitude_ch128[26]), + std::sin(scan_laser_altitude_ch128[27]), + std::sin(scan_laser_altitude_ch128[28]), + std::sin(scan_laser_altitude_ch128[29]), + std::sin(scan_laser_altitude_ch128[30]), + std::sin(scan_laser_altitude_ch128[31]), +}; + +static const double scan_mirror_altitude[4] = { + -0.0, + 0.005759586531581287, + 0.011693705988362009, + 0.017453292519943295, +}; + +static const double sin_scan_mirror_altitude[4] = { + std::sin(scan_mirror_altitude[0]), + std::sin(scan_mirror_altitude[1]), + std::sin(scan_mirror_altitude[2]), + std::sin(scan_mirror_altitude[3]), +}; + +static const double scan_mirror_altitude_ch128[4] = { + 0.0, + 0.004363323129985824, + 0.008726646259971648, + 0.013089969389957472, +}; + +static const double sin_scan_mirror_altitude_ch128[4] = { + std::sin(scan_mirror_altitude_ch128[0]), + std::sin(scan_mirror_altitude_ch128[1]), + std::sin(scan_mirror_altitude_ch128[2]), + std::sin(scan_mirror_altitude_ch128[3]), +}; + +static const int POINTS_PER_PACKET_SINGLE_ECHO = 1192; // modify +static const int POINTS_PER_PACKET_DOUBLE_ECHO = 1188; // modify + +/** \brief Raw Lslidar packet. + * + * revolution is described in the device manual as incrementing + * (mod 65536) for each physical turn of the device. Our device + * seems to alternate between two different values every third + * packet. One value increases, the other decreases. + * + * status has either a temperature encoding or the microcode level + */ +typedef struct FiringC32 { + uint16_t firing_azimuth[BLOCKS_PER_PACKET]; + uint16_t azimuth[SCANS_PER_PACKET]; + double distance[SCANS_PER_PACKET]; + double intensity[SCANS_PER_PACKET]; +} FiringC32; + +union TwoBytes { + uint16_t distance; + uint8_t bytes[2]; +}; + +struct RawBlock { + uint16_t header; + uint16_t rotation; // 0-35999 + uint8_t data[BLOCK_DATA_SIZE]; +}; + +struct RawPacket_C32 { + RawBlock blocks[BLOCKS_PER_PACKET]; + uint32_t time_stamp; + uint8_t factory[2]; +}; + +typedef struct raw_packet { + raw_block_t blocks[BLOCKS_PER_PACKET]; + uint8_t gps_timestamp[10]; // gps时间 1200-1209 + uint8_t status_type; + uint8_t status_value; +} raw_packet_t; + +union vertical_point { + uint8_t uint[2]; + uint16_t value; +}; + +struct Point { + uint8_t vertical_line; // 0-127 + uint8_t azimuth_1; + uint8_t azimuth_2; + uint8_t distance_1; + uint8_t distance_2; + uint8_t distance_3; + uint8_t intensity; +}; + +struct RawPacket { + Point points[POINTS_PER_PACKET]; + uint8_t nodata[3]; + uint32_t time_stamp; + uint8_t factory[2]; +}; + +struct Firing { + // double vertical_angle; + int vertical_line; + double azimuth; + double distance; + int intensity; +}; + +struct Firing_LS128S2 { + double vertical_angle; + // int vertical_line; + double azimuth; + double distance; + float intensity; + double time; + int channel_number; +}; + +struct Point_LS128S2 { + uint8_t vertical_line; // 0-127 + uint8_t azimuth_1; + uint8_t azimuth_2; + uint8_t distance_1; + uint8_t distance_2; + uint8_t distance_3; + uint8_t intensity; +}; + +struct RawPacket_LS128S2 { + Point points[POINTS_PER_PACKET]; + uint8_t nodata[3]; + uint32_t time_stamp; + uint8_t factory[2]; +}; + +static const float nan = std::numeric_limits::signaling_NaN(); + +/** \brief Lslidar data conversion class */ +class LslidarParser { + public: + LslidarParser() {} + explicit LslidarParser(const Config& config); + virtual ~LslidarParser() {} + + /** \brief Set up for data processing. + * Perform initializations needed before data processing can + * begin: + * - read device-specific angles calibration + * + * @param private_nh private node handle for ROS parameters + * @returns 0 if successful; + * errno value for failure + */ + virtual void GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg) = 0; + virtual void setup(); + + // Order point cloud fod IDL by lslidar model + virtual void Order(std::shared_ptr cloud) = 0; + + const double get_last_timestamp() { return last_time_stamp_; } + + 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; + + 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); +}; // class LslidarParser + +class Lslidar16Parser : public LslidarParser { + public: + explicit Lslidar16Parser(const Config& config); + ~Lslidar16Parser() {} + + void GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg); + void Order(std::shared_ptr cloud); + + private: + void Unpack(const LslidarPacket& pkt, + std::shared_ptr pc, + int packet_number); + // Previous Lslidar packet time stamp. (offset to the top hour) + double previous_packet_stamp_; + uint64_t gps_base_usec_; // full time + uint64_t last_packet_time; + uint8_t difop_data[PACKET_SIZE]; + int block_num; + int packet_number_; + bool read_difop_; +}; // class Lslidar16Parser + +class Lslidar32Parser : public LslidarParser { + public: + explicit Lslidar32Parser(const Config& config); + ~Lslidar32Parser() {} + + void GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg); + void Order(std::shared_ptr cloud); + + private: + void Unpack(const LslidarPacket& pkt, + std::shared_ptr pc, + int packet_number); + double adjust_angle; + double adjust_angle_two; + double adjust_angle_three; + double adjust_angle_four; + + // Previous Lslidar packet time stamp. (offset to the top hour) + double previous_packet_stamp_; + uint64_t gps_base_usec_; // full time + uint64_t last_packet_time; + uint64_t last_point_time; + uint8_t difop_data[PACKET_SIZE]; + int block_num; + int lslidar_type; + int packet_number_; + bool read_difop_; + + double scan_altitude_A[32] = {0}; + double scan_altitude_C[32] = {0}; +}; // class Lslidar32Parser + +class LslidarCXV4Parser : public LslidarParser { + public: + explicit LslidarCXV4Parser(const Config& config); + ~LslidarCXV4Parser() {} + + void decodePacket(const RawPacket_C32* packet); + + void GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg); + void Order(std::shared_ptr cloud); + + bool checkPacketValidity(const RawPacket_C32* packet); + + private: + void Unpack(const LslidarPacket& pkt, + std::shared_ptr pc, + int packet_number); + + // Previous Lslidar packet time stamp. (offset to the top hour) + double previous_packet_stamp_; + uint64_t gps_base_usec_; // full time + uint64_t last_packet_time; + uint64_t last_point_time; + uint8_t difop_data[PACKET_SIZE]; + int block_num; + int lslidar_type; + int packet_number_; + bool read_difop_; + + // double c32_vertical_angle[32] = {0}; + double cos_scan_altitude[32] = {0}; + double sin_scan_altitude[32] = {0}; + double cos_azimuth_table[36000]; + double sin_azimuth_table[36000]; + int vertical_angle_; // 雷达垂直角度： 32度，70度，90度 + FiringC32 firings; + double distance_unit_; + int lidar_number_; + bool is_new_c32w_; + bool is_get_scan_altitude_; +}; // class LslidarCXV4Parser + +class Lslidar401Parser : public LslidarParser { + public: + explicit Lslidar401Parser(const Config& config); + ~Lslidar401Parser() {} + + void GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg); + void Order(std::shared_ptr cloud); + + private: + void decodePacket(const raw_packet_t* packet); + void Unpack(const LslidarPacket& pkt, + std::shared_ptr pc); + + // Previous Lslidar packet time stamp. (offset to the top hour) + double previous_packet_stamp_; + uint64_t gps_base_usec_; // full time + uint64_t last_packet_time; + uint8_t difop_data[PACKET_SIZE]; + int block_num; +}; // class Lslidar401Parser + +class LslidarCH16Parser : public LslidarParser { + public: + explicit LslidarCH16Parser(const Config& config); + ~LslidarCH16Parser() {} + + void GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg); + void Order(std::shared_ptr cloud); + + private: + void Unpack(int num, const LslidarPacket& pkt, + std::shared_ptr pc); + + // Previous Lslidar packet time stamp. (offset to the top hour) + double previous_packet_stamp_; + uint64_t gps_base_usec_; // full time + uint64_t last_packet_time; + uint64_t last_point_time; + size_t packets_size; + uint64_t time_last; + uint8_t difop_data[PACKET_SIZE]; + Firing firings[POINTS_PER_PACKET]; +}; // class LslidarCH16Parser + +class LslidarCH32Parser : public LslidarParser { + public: + explicit LslidarCH32Parser(const Config& config); + ~LslidarCH32Parser() {} + + void GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg); + void Order(std::shared_ptr cloud); + + private: + void Unpack(int num, const LslidarPacket& pkt, + std::shared_ptr pc); + + // Previous Lslidar packet time stamp. (offset to the top hour) + double previous_packet_stamp_; + uint64_t gps_base_usec_; // full time + uint64_t last_packet_time; + uint64_t last_point_time; + size_t packets_size; + uint64_t time_last; + uint8_t difop_data[PACKET_SIZE]; + Firing firings[POINTS_PER_PACKET]; +}; // class LslidarCH32Parser + +class LslidarCH64Parser : public LslidarParser { + public: + explicit LslidarCH64Parser(const Config& config); + ~LslidarCH64Parser() {} + + void GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg); + void Order(std::shared_ptr cloud); + + private: + void Unpack(int num, const LslidarPacket& pkt, + std::shared_ptr pc); + + // Previous Lslidar packet time stamp. (offset to the top hour) + double previous_packet_stamp_; + uint64_t gps_base_usec_; // full time + uint64_t last_packet_time; + uint64_t last_point_time; + size_t packets_size; + uint64_t time_last; + uint8_t difop_data[PACKET_SIZE]; + Firing firings[POINTS_PER_PACKET]; +}; // class LslidarCH64Parser + +class LslidarCH64wParser : public LslidarParser { + public: + explicit LslidarCH64wParser(const Config& config); + ~LslidarCH64wParser() {} + + void GeneratePointcloud( + const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg); + void Order(std::shared_ptr cloud); + + private: + void Unpack(int num, const LslidarPacket& pkt, + std::shared_ptr pc); + + // Previous Lslidar packet time stamp. (offset to the top hour) + double previous_packet_stamp_; + uint64_t gps_base_usec_; // full time + uint64_t last_packet_time; + uint64_t last_point_time; + size_t packets_size; + uint64_t time_last; + double theat1_s[128]; + double theat2_s[128]; + double theat1_c[128]; + double theat2_c[128]; + double point_time_diff; + uint8_t difop_data[PACKET_SIZE]; + Firing firings[POINTS_PER_PACKET]; +}; // class LslidarCH64Parser + +class LslidarCH120Parser : public LslidarParser { + public: + explicit LslidarCH120Parser(const Config& config); + ~LslidarCH120Parser() {} + + void GeneratePointcloud(const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg); + void Order(std::shared_ptr cloud); + + private: + void Unpack(int num, const LslidarPacket& pkt, + std::shared_ptr pc); + + // Previous Lslidar packet time stamp. (offset to the top hour) + double previous_packet_stamp_; + uint64_t gps_base_usec_; // full time + uint64_t last_packet_time; + uint64_t last_point_time; + size_t packets_size; + uint64_t time_last; + uint8_t difop_data[PACKET_SIZE]; + Firing firings[POINTS_PER_PACKET]; +}; // class LslidarCH120Parser + +class LslidarCH128Parser : public LslidarParser { + public: + explicit LslidarCH128Parser(const Config& config); + ~LslidarCH128Parser() {} + + void GeneratePointcloud(const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg); + void Order(std::shared_ptr cloud); + + private: + void Unpack(int num, const LslidarPacket& pkt, + std::shared_ptr pc); + + // Previous Lslidar packet time stamp. (offset to the top hour) + double previous_packet_stamp_; + uint64_t gps_base_usec_; // full time + uint64_t last_packet_time; + uint64_t last_point_time; + size_t packets_size; + double sinTheta_2[128]; + double prism_angle128[4]; + uint64_t time_last; + uint8_t difop_data[PACKET_SIZE]; + Firing firings[POINTS_PER_PACKET]; +}; // class LslidarCH128Parser + +class LslidarCH128X1Parser : public LslidarParser { + public: + explicit LslidarCH128X1Parser(const Config& config); + ~LslidarCH128X1Parser() {} + + void GeneratePointcloud(const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg); + void Order(std::shared_ptr cloud); + + private: + void Unpack(int num, const LslidarPacket& pkt, + std::shared_ptr pc); + + // Previous Lslidar packet time stamp. (offset to the top hour) + double previous_packet_stamp_; + uint64_t gps_base_usec_; // full time + uint64_t last_packet_time; + uint64_t last_point_time; + size_t packets_size; + uint8_t difop_data[PACKET_SIZE]; + Firing firings[POINTS_PER_PACKET]; +}; // class LslidarCH128X1Parser + +class LslidarLS128S2Parser : public LslidarParser { + public: + explicit LslidarLS128S2Parser(const Config& config); + ~LslidarLS128S2Parser() {} + + void GeneratePointcloud(const std::shared_ptr& scan_msg, + const std::shared_ptr& out_msg); + void Order(std::shared_ptr cloud); + + int convertCoordinate(const struct Firing_LS128S2& lidardata); + + int convertCoordinate(const struct Firing_LS128S2& lidardata, + std::shared_ptr out_cloud); + + private: + void Unpack(int num, const LslidarPacket& pkt, + std::shared_ptr pc); + + // Previous Lslidar packet time stamp. (offset to the top hour) + double previous_packet_stamp_; + uint64_t gps_base_usec_; // full time + uint64_t last_packet_time; + uint64_t last_point_time; + size_t packets_size; + uint8_t difop_data[PACKET_SIZE]; + Firing_LS128S2 firings[LS_POINTS_PER_PACKET]; + int frame_count = 0; // LS系列，叠帧模式会用到 + + double cos_table[36000]{}; + double sin_table[36000]{}; + double cos_mirror_angle[4]{}; + double sin_mirror_angle[4]{}; + double g_fAngleAcc_V = 0.01; + bool is_add_frame_ = false; + int return_mode = 1; + std::shared_ptr cur_pc; + std::shared_ptr pre_pc; + int packet_number_ = 0; +}; // class LslidarLS128S2Parser + +class LslidarParserFactory { + public: + static LslidarParser* CreateParser(Config config); +}; + +} // namespace lslidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/lslidar/proto/BUILD b/modules/drivers/lidar/lslidar/proto/BUILD new file mode 100644 index 00000000000..426aa8d30f4 --- /dev/null +++ b/modules/drivers/lidar/lslidar/proto/BUILD @@ -0,0 +1,52 @@ +## Auto generated by `proto_build_generator.py` +load("@rules_proto//proto:defs.bzl", "proto_library") +load("@rules_cc//cc:defs.bzl", "cc_proto_library") +load("//tools:python_rules.bzl", "py_proto_library") + +package(default_visibility = ["//visibility:public"]) + +cc_proto_library( + name = "config_cc_proto", + deps = [ + ":config_proto", + ], +) + +proto_library( + name = "config_proto", + srcs = ["config.proto"], + deps = [ + ":lslidar_proto", + ], +) + +py_proto_library( + name = "config_py_pb2", + deps = [ + ":config_proto", + ":lslidar_py_pb2", + ], +) + +cc_proto_library( + name = "lslidar_cc_proto", + deps = [ + ":lslidar_proto", + ], +) + +proto_library( + name = "lslidar_proto", + srcs = ["lslidar.proto"], + deps = [ + "//modules/common_msgs/basic_msgs:header_proto", + ], +) + +py_proto_library( + name = "lslidar_py_pb2", + deps = [ + ":lslidar_proto", + "//modules/common_msgs/basic_msgs:header_py_pb2", + ], +) diff --git a/modules/drivers/lidar/lslidar/proto/config.proto b/modules/drivers/lidar/lslidar/proto/config.proto new file mode 100644 index 00000000000..24221d6c936 --- /dev/null +++ b/modules/drivers/lidar/lslidar/proto/config.proto @@ -0,0 +1,47 @@ +syntax = "proto2"; + +package apollo.drivers.lslidar; + +import "modules/drivers/lidar/lslidar/proto/lslidar.proto"; + +message Config { + optional Model model = 1; + optional string device_ip = 2 [default = "192.168.1.200"]; + optional uint32 msop_port = 3; + optional uint32 difop_port = 4; + optional uint32 return_mode = 5; + optional uint32 degree_mode = 6; + optional float distance_unit = 7; + optional bool time_synchronization = 8; + optional bool add_multicast = 9; + optional string group_ip = 10 [default = "224.1.1.2"]; + optional uint32 rpm = 11; + optional string convert_channel_name = 12; + optional uint32 time_zone = 13; + optional string frame_id = 14; + optional string scan_channel = 15; + optional float min_range = 16; + optional float max_range = 17; + optional bool config_vert = 18; + optional bool print_vert = 19; + optional float scan_start_angle =20; + optional float scan_end_angle = 21; + optional bool calibration = 22; + optional int32 npackets = 23; + optional string calibration_file = 24; + optional uint32 packet_size = 25; + optional uint32 vertical_angle = 26; + optional float bottom_left_x = 27; + optional float bottom_left_y = 28; + optional float top_right_x = 29; + optional float top_right_y = 30; + optional string pcap_path = 31; +} + +message CompensatorConfig { + optional string output_channel = 1; + optional float transform_query_timeout = 2 [default = 0.02]; + optional string world_frame_id = 3 [default = "world"]; + optional string target_frame_id = 4; + optional uint32 point_cloud_size = 5; +} \ No newline at end of file diff --git a/modules/drivers/lidar/lslidar/proto/lslidar.proto b/modules/drivers/lidar/lslidar/proto/lslidar.proto new file mode 100644 index 00000000000..bec13d14423 --- /dev/null +++ b/modules/drivers/lidar/lslidar/proto/lslidar.proto @@ -0,0 +1,40 @@ +syntax = "proto2"; + +package apollo.drivers.lslidar; + +// import "modules/common/proto/header.proto"; +import "modules/common_msgs/basic_msgs/header.proto"; + + +enum Model { + UNKOWN = 0; + LSLIDAR16P = 1; + LSLIDAR32P = 2; + LSLIDAR401 = 3; //401 废弃不用 + LSLIDAR_CH16 = 4; + LSLIDAR_CH32 = 5; + LSLIDAR_CH64 = 6; + LSLIDAR_CH64w = 7; + LSLIDAR_CH120 = 8; + LSLIDAR_CH128 = 9; + LSLIDAR_CH128X1 = 10; + LSLIDAR_C32_V4 = 11; + LSLIDAR_C16_V4 = 12; + LSLIDAR_C8_V4 = 13; + LSLIDAR_C1_V4 = 14; + LSLIDAR_LS128S2 = 15; +} + +message LslidarPacket { + optional uint64 stamp = 1; + optional bytes data = 2; +} + +message LslidarScan { + optional apollo.common.Header header = 1; + optional Model model = 2; + repeated LslidarPacket firing_pkts = 3; + repeated LslidarPacket difop_pkts = 4; + optional string sn = 5; + optional uint64 basetime = 6 [default = 0]; +}