Skip to content

Commit 1020912

Browse files
yuxin-2yuxin-2
committed
fix:lslidar_driver and add_lsdriver_fix_readme
1 parent c48541b commit 1020912

19 files changed

+346
-63
lines changed

‎modules/drivers/lidar/lslidar/DRIVER_FIX_README.md‎

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
# 镭神Apollo修改
2+
3+
4+
5+
### lslidar_component.cpp
6+
7+
~~~c++
8+
void LslidarComponent::ScanQueuePollProcess()
9+
10+
// 更改判断条件，!apollo::cyber::IsShutdown()与 scan_queue_.popWait(scan_frame)分开
11+
// 一起判断会导致点云发布信息异常
12+
13+
14+
void LslidarComponent::HandleScanFrame(const std::shared_ptr<LslidarScan> &scan_frame)
15+
16+
// 增加时间设置
17+
// timestamp_sec 为系统时间
18+
// lidar_timestamp，measurement_time 为每帧最后一个点时间
19+
~~~
20+
21+
22+
23+
### driver.cc
24+
25+
~~~c++
26+
void LslidarDriver::Init()
27+
28+
// 增大LSLIDAR_LS128S2 packets_rate值，避免LS系列雷达实际包数比npackets大导致点云帧率翻倍
29+
// 应该放弃scan->firing_pkts_size() < config_.npackets()判断，仅用包头或角度判断一帧起始
30+
~~~
31+
32+
33+
34+
### lslidarCXV4_parser.cc
35+
36+
~~~c++
37+
void LslidarCXV4Parser::Unpack(const LslidarPacket &pkt,std::shared_ptr<PointCloud> pc, int packet_number)
38+
39+
// 增加当前包的时间 current_packet_time = pkt.stamp(); 旧代码中没有给当包赋时间，导致机械式4.0雷达点的时间均为0
40+
// 修改无效点，由于机械式是有序点云，所有线号点数需一致，无效点均赋值为nan。现在将机械式点云修改为无序点云，无效点直接continue过滤，不在添加进点云。
41+
42+
43+
void LslidarCXV4Parser::Order(std::shared_ptr<PointCloud> cloud)
44+
45+
// 注释点云排序功能，无序点云不需要排序
46+
~~~
47+
48+
49+
50+
### lslidarLS128S2_parser.cc
51+
52+
~~~c++
53+
LslidarLS128S2Parser::LslidarLS128S2Parser(const Config &config)
54+
55+
// 更新LS系列雷达摆镜角度 旧: {0, -2, -1, -3}
56+
// double mirror_angle[4] = {1.5, -0.5, 0.5, -1.5}; // 新摆镜角度
57+
~~~
58+
59+
60+
61+
62+
63+
机械式点云改为无序点云，height为1，width为该帧点数。
64+
65+
增加多台雷达运行示例 lslidar_four.dag lslidar_four.launch
66+
67+
注释的AERROR日志可以删除，仅用于调试
68+
69+
70+
71+
后续可更新:
72+
73+
- 兼容新款雷达
74+
- 增加LS系列雷达角度畸变矫正解析
75+
- 点云旋转平移功能
76+
77+
78+
79+
80+

‎modules/drivers/lidar/lslidar/component/lslidar_component.cpp‎

Lines changed: 24 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -71,12 +71,12 @@ void LslidarComponent::DevicePollProcess() {
7171
bool ret = dvr_->Poll(scan);
7272
if (ret) {
7373
common::util::FillHeader("lslidar", scan.get());
74-
AINFO << "publish scan!";
75-
double time1 = apollo::cyber::Time().Now().ToSecond();
74+
// AINFO << "publish scan!";
75+
// double time1 = apollo::cyber::Time().Now().ToSecond();
7676
this->WriteScan(scan);
77-
double time2 = apollo::cyber::Time().Now().ToSecond();
78-
AINFO << "apollo::cyber::Time((time2 - time1)"
79-
<< apollo::cyber::Time((time2 - time1) / 2.0).ToNanosecond();
77+
// double time2 = apollo::cyber::Time().Now().ToSecond();
78+
// AINFO << "apollo::cyber::Time((time2 - time1)"
79+
// << apollo::cyber::Time((time2 - time1) / 2.0).ToNanosecond();
8080
scan_queue_.push(scan);
8181
} else {
8282
AWARN << "device poll failed";
@@ -91,6 +91,8 @@ void LslidarComponent::ScanQueuePollProcess() {
9191
while (!apollo::cyber::IsShutdown()) {
9292
if (scan_queue_.popWait(scan_frame)) {
9393
HandleScanFrame(scan_frame);
94+
} else {
95+
std::this_thread::sleep_for(std::chrono::milliseconds(1));
9496
}
9597
}
9698
}
@@ -100,6 +102,23 @@ void LslidarComponent::HandleScanFrame(
100102
std::shared_ptr<apollo::drivers::PointCloud> point_cloud_out =
101103
this->AllocatePointCloud();
102104
converter_->ConvertPacketsToPointcloud(scan_frame, point_cloud_out);
105+
106+
// double first_point_time = point_cloud_out->point(0).timestamp();
107+
double last_point_time = point_cloud_out->point(point_cloud_out->point_size() - 1).timestamp();
108+
109+
point_cloud_out->mutable_header()->set_timestamp_sec(cyber::Time().Now().ToSecond()); // 系统时间
110+
point_cloud_out->mutable_header()->set_lidar_timestamp(last_point_time); // 最后一个点的时间
111+
point_cloud_out->set_measurement_time(last_point_time * 1e-9); // 最后一个点的时间
112+
113+
// double timestamp_sec = point_cloud_out->header().timestamp_sec();
114+
// std::ostringstream oss;
115+
// oss << std::fixed << std::setprecision(9) << timestamp_sec;
116+
117+
// AERROR << "Point Cloud Time: " << oss.str();
118+
// AERROR << "First Point Time: " << point_cloud_out->point(0).timestamp();
119+
// AERROR << " Last Point Time: " << point_cloud_out->point(point_cloud_out->point().size() - 1).timestamp();
120+
// AERROR << "";
121+
103122
this->WritePointCloud(point_cloud_out);
104123
}
105124

‎modules/drivers/lidar/lslidar/conf/C32W_left_conf.pb.txt‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
config_base {
2+
scan_channel: "/apollo/sensor/C32W_left/Scan"
3+
point_cloud_channel: "/apollo/sensor/C32W_left/PointCloud2"
4+
frame_id: "C32W_left"
5+
6+
# sample ONLINE_LIDAR, RAW_PACKET
7+
source_type: ONLINE_LIDAR
8+
}
9+
10+
model: LSLIDAR_C32_V4
11+
device_ip: "127.0.0.1"
12+
msop_port: 2008
13+
difop_port: 2009
14+
vertical_angle: 70 # 雷达垂直角度： 32度，70度，90度
15+
return_mode: 1
16+
degree_mode: 2 #2: 均匀1度校准两列 1://均匀0.33度校准两列
17+
distance_unit: 0.4
18+
packet_size: 1212
19+
time_synchronization: false
20+
add_multicast: false
21+
group_ip: "224.1.1.2"
22+
rpm: 600
23+
frame_id: "C32W_left"
24+
min_range: 0.15
25+
max_range: 150.0
26+
config_vert: true
27+
scan_start_angle: 0.0
28+
scan_end_angle: 36000.0
29+
#要屏蔽的矩形区域参数
30+
bottom_left_x: 0.0 #左下 x值
31+
bottom_left_y: 0.0 #左下 y值
32+
top_right_x: 0.0 #右上 x值
33+
top_right_y: 0.0 #右上 y值
34+
pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空

‎modules/drivers/lidar/lslidar/conf/C32W_right_conf.pb.txt‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
config_base {
2+
scan_channel: "/apollo/sensor/C32W_right/Scan"
3+
point_cloud_channel: "/apollo/sensor/C32W_right/PointCloud2"
4+
frame_id: "C32W_right"
5+
6+
# sample ONLINE_LIDAR, RAW_PACKET
7+
source_type: ONLINE_LIDAR
8+
}
9+
10+
model: LSLIDAR_C32_V4
11+
device_ip: "192.168.1.201"
12+
msop_port: 2370
13+
difop_port: 2371
14+
vertical_angle: 70 # 雷达垂直角度： 32度，70度，90度
15+
return_mode: 1
16+
degree_mode: 2 #2: 均匀1度校准两列 1://均匀0.33度校准两列
17+
distance_unit: 0.4
18+
packet_size: 1212
19+
time_synchronization: false
20+
add_multicast: false
21+
group_ip: "224.1.1.2"
22+
rpm: 600
23+
frame_id: "C32W_right"
24+
min_range: 0.15
25+
max_range: 150.0
26+
config_vert: true
27+
scan_start_angle: 0.0
28+
scan_end_angle: 36000.0
29+
#要屏蔽的矩形区域参数
30+
bottom_left_x: 0.0 #左下 x值
31+
bottom_left_y: 0.0 #左下 y值
32+
top_right_x: 0.0 #右上 x值
33+
top_right_y: 0.0 #右上 y值
34+
pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空

‎modules/drivers/lidar/lslidar/conf/LS180S3_back_conf.pb.txt‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
config_base {
2+
scan_channel: "/apollo/sensor/LS180S3_back/Scan"
3+
point_cloud_channel: "/apollo/sensor/LS180S3_back/PointCloud2"
4+
frame_id: "LS180S3_back"
5+
6+
# sample ONLINE_LIDAR, RAW_PACKET
7+
source_type: ONLINE_LIDAR
8+
}
9+
10+
model: LSLIDAR_LS128S2
11+
device_ip: "192.168.1.203"
12+
msop_port: 2374
13+
difop_port: 2375
14+
packet_size: 1206
15+
time_synchronization: false
16+
add_multicast: false
17+
group_ip: "224.1.1.2"
18+
rpm: 600
19+
frame_id: "LS180S3_back"
20+
min_range: 0.15
21+
max_range: 500.0
22+
scan_start_angle: -60.0
23+
scan_end_angle: 60.0
24+
calibration: false
25+
calibration_file: "modules/drivers/lidar/lslidar/params/LS128S2_calibration.yaml"
26+
27+
#要屏蔽的矩形区域参数
28+
bottom_left_x: 0.0 #左下 x值
29+
bottom_left_y: 0.0 #左下 y值
30+
top_right_x: 0.0 #右上 x值
31+
top_right_y: 0.0 #右上 y值
32+
pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空

‎modules/drivers/lidar/lslidar/conf/LS180S3_front_conf.pb.txt‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
config_base {
2+
scan_channel: "/apollo/sensor/LS180S3_front/Scan"
3+
point_cloud_channel: "/apollo/sensor/LS180S3_front/PointCloud2"
4+
frame_id: "LS180S3_front"
5+
6+
# sample ONLINE_LIDAR, RAW_PACKET
7+
source_type: ONLINE_LIDAR
8+
}
9+
10+
model: LSLIDAR_LS128S2
11+
device_ip: "127.0.0.1"
12+
msop_port: 2368
13+
difop_port: 2369
14+
packet_size: 1206
15+
time_synchronization: false
16+
add_multicast: false
17+
group_ip: "224.1.1.2"
18+
rpm: 600
19+
frame_id: "LS180S3_front"
20+
min_range: 0.15
21+
max_range: 500.0
22+
scan_start_angle: -60.0
23+
scan_end_angle: 60.0
24+
calibration: false
25+
calibration_file: "modules/drivers/lidar/lslidar/params/LS128S2_calibration.yaml"
26+
27+
#要屏蔽的矩形区域参数
28+
bottom_left_x: 0.0 #左下 x值
29+
bottom_left_y: 0.0 #左下 y值
30+
top_right_x: 0.0 #右上 x值
31+
top_right_y: 0.0 #右上 y值
32+
pcap_path: "" #读取pcap包，测试时使用，连接雷达的时候，将此值设置为空

‎modules/drivers/lidar/lslidar/conf/lslidar16v4_conf.pb.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ config_base {
88
}
99

1010
model: LSLIDAR_C16_V4
11-
device_ip: "192.168.1.200"
11+
device_ip: "127.0.0.1"
1212
msop_port: 2368
1313
difop_port: 2369
1414
return_mode: 1

‎modules/drivers/lidar/lslidar/conf/lslidar32v4_conf.pb.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ config_base {
88
}
99

1010
model: LSLIDAR_C32_V4
11-
device_ip: "192.168.1.200"
11+
device_ip: "127.0.0.1"
1212
msop_port: 2368
1313
difop_port: 2369
1414
vertical_angle: 32 # 雷达垂直角度： 32度，70度，90度

‎modules/drivers/lidar/lslidar/conf/lslidarCH128X1_conf.pb.txt‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -8,9 +8,9 @@ config_base {
88
}
99

1010
model: LSLIDAR_CH128X1
11-
device_ip: "192.168.1.200"
12-
msop_port: 2368
13-
difop_port: 2369
11+
device_ip: "127.0.0.1"
12+
msop_port: 2372
13+
difop_port: 2373
1414
packet_size: 1206
1515
time_synchronization: false
1616
add_multicast: false

‎modules/drivers/lidar/lslidar/conf/lslidarLS128S2_conf.pb.txt‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -8,9 +8,9 @@ config_base {
88
}
99

1010
model: LSLIDAR_LS128S2
11-
device_ip: "192.168.1.200"
12-
msop_port: 2368
13-
difop_port: 2369
11+
device_ip: "127.0.0.1"
12+
msop_port: 2374
13+
difop_port: 2375
1414
packet_size: 1206
1515
time_synchronization: false
1616
add_multicast: false

0 commit comments

Comments
 (0)