初识Airsim(十)之Lidar数据获取并显示

前言

在Airsim若要使用Lidar传感器并进行显示,一般都离不开rviz,使用rviz对ros的topic进行显示,但也因此,需要先将数据封装成ros的消息类型,我没有采用官方的airsim_ros包,而是自己进行封装创建。

那么目前导航功能包集只接受使用sensor_msgs/LaserScan或sensor_msgs/PointCloud及新出来的sensor_msgs/PointCloud2消息类型发布的传感器数据。

但我在google或者百度上也搜寻了很多LaserScan和PointCloud之间的区别,没怎么找到,更多的都是介绍如何使用这两个类型去发布ros数据。因此,也只有根据自己的理解去记录。

一、Airsim添加Lidar传感器

详情请点击官方Lidar文档

Parameter Description
NumberOfChannels 激光雷达线束(单线或多线),默认为16线
Range 扫描范围(单位米)
PointsPerSecond 每秒捕获的点数
RotationsPerSecon 每秒轮换数
HorizontalFOVStart 水平起始角度(以度为单位)
HorizontalFOVEnd 水平结束角度(以度为单位)
VerticalFOVUpper Vertical FOV upper limit for the lidar, in degrees
VerticalFOVLower 垂直角度下限(以度为单位)
X Y Z 激光雷达相对于车辆的位置(NED坐标,米为单位)
Roll Pitch Yaw 激光雷达相对车车辆的方向(以度为单位)
DataFrame 输出中的点的框架
那么**线束**到底代表什么意思呢?单线和多线又有什么区别?

浅谈激光雷达一文中解释了相关线束的含义,通过浏览,个人理解为:每个线束每秒捕获的点数都有个最大值,如100000,那么多线则能捕获更多的点数,因此多线可以捕获到上百万的点数。单线可以表示为单个圆,多线可以有多个圆。

且多线激光中一般垂直方向角度的范围为40度(不超过40度),忘记是在哪篇文章上看到过

1.1 settings.json默认配置

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
32
33
34
35
36
37
38
39
40
41
42
43
{
"SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings_json.md",
"SettingsVersion": 1.2,

"SimMode": "Multirotor",

"Vehicles": {
"Drone1": {
"VehicleType": "simpleflight",
"AutoCreate": true,
"Sensors": {
"LidarSensor1": {
"SensorType": 6, # 6表示使用激光雷达传感器
"Enabled" : true,
"NumberOfChannels": 16, # 16线激光
"RotationsPerSecond": 10,
"PointsPerSecond": 100000,
"X": 0, "Y": 0, "Z": -1,
"Roll": 0, "Pitch": 0, "Yaw" : 0, # 定义位姿姿态
"VerticalFOVUpper": -15,
"VerticalFOVLower": -25, # 垂直方向角度范围
"HorizontalFOVStart": -20,
"HorizontalFOVEnd": 20, # 水平方向角度范围
"DrawDebugPoints": true, # 是否在环境中可视
"DataFrame": "SensorLocalFrame" #垂直惯性坐标系"VehicleInertialFrame" or 传感器坐标系"SensorLocalFrame"
},
"LidarSensor2": { # 可以定义多个雷达
"SensorType": 6,
"Enabled" : true,
"NumberOfChannels": 4,
"RotationsPerSecond": 10,
"PointsPerSecond": 10000,
"X": 0, "Y": 0, "Z": -1,
"Roll": 0, "Pitch": 0, "Yaw" : 0,
"VerticalFOVUpper": -15,
"VerticalFOVLower": -25,
"DrawDebugPoints": true,
"DataFrame": "SensorLocalFrame"
}
}
}
}
}

如果调用getLidarData()函数,则将返回点云数组、时间戳和雷达位姿,其中:

点云在雷达坐标系中(NED坐标系,以米为单位)

雷达位姿在车的坐标系中(NED坐标系,以米为单位)

1.2 简单json配置,测试

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
{
"SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
"SettingsVersion": 1.2,
"SimMode":"Multirotor",

"Vehicles":{
"Drone1":{
"VehicleType":"SimpleFlight",
"X":0,"Y":0,"Z":0, # 设定无人机的初始坐标
"Sensors":
{
"MyLidar1":
{
"SensorType":6, # 激光雷达传感器为6
"Enabled":true,
"NumberOfChannels":16, # 16线激光
"PointsPerSecond":10000,
"X":0,"Y":0,"Z":-1,
"DrawDebugPoints":true,
"Roll": 0, "Pitch": 0, "Yaw" : 0, # 定义了雷达相对于无人机的位姿
"VerticalFOVUpper": 0,
"VerticalFOVLower": 0, # 垂直角度,一般不超过40度
"HorizontalFOVStart": -20,
"HorizontalFOVEnd": 20, # 水平角度范围
"DrawDebugPoints": true, # 是否在环境中可视
"DataFrame": "SensorLocalFrame" #垂直惯性坐标系"VehicleInertialFrame" or 传感器坐标系"SensorLocalFrame"
}
}
}
}
}

环境中的显示(第一视角)

Python代码编写,获取雷达数据

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
#!/usr/bin/env python
# -*-coding:utf-8 -*-

import cv2
import numpy as np

import airsim
import time
import datetime
import pprint



client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)

lidarData = client.getLidarData()
print('lidar',lidarData)

if len(lidarData.point_cloud) >3:
points = np.array(lidarData.point_cloud,dtype=np.dtype('f4'))
points = np.reshape(points,(int(points.shape[0]/3),3))
print('number of points'),len(points)
else:
print("\tNo points received from Lidar data")

这里不展示输出内容,因为输出内容比较多,占位置,在每次运行时,其获得的雷达点的个数可能会稍有不同,并且雷达探测到的点也会不一样,不相同才是正常的。

二、单线激光雷达设置

2.1 settings.json设置

为什么要将垂直角度设置为0?因为个人理解单线是水平的,即2维的,如果设置了垂直角度,则将变成三维,后面会显示设置了垂直角度后的区别

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
32
{
"SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
"SettingsVersion": 1.2,
"SimMode":"Multirotor",

"Vehicles":{
"Drone1":{
"VehicleType":"SimpleFlight",
"X":0,"Y":0,"Z":0,
"Roll": 0, "Pitch": 0, "Yaw" : 0,
"Sensors":
{
"MyLidar1":
{
"SensorType":6,
"Enabled":true,
"NumberOfChannels":1, # 设置为单线
"PointsPerSecond":10000, # 10000个数据点
"X":0,"Y":0,"Z":-0.5,
"DrawDebugPoints":true,
"Roll": 0, "Pitch": 0, "Yaw" : 0,
"VerticalFOVUpper": 0,
"VerticalFOVLower": 0, # 垂直角度为0
"HorizontalFOVStart": -90,
"HorizontalFOVEnd": 90, # 水平180度
"DrawDebugPoints": true,
"DataFrame": "SensorLocalFrame"
}
}
}
}
}

测试环境

2.2 Python代码编写(LaserScan)

LaserScan是一个二维结构,即垂直角度为0

LaserScan消息定义官方定义

1
2
3
4
5
6
7
8
9
10
11
12
13
14
Header head:
uint32 seq //对应一个标识符,随着消息被发布,它会自动增加
time stamp //时间戳,以激光扫描为例,stamp可能对应每次扫描开始的时间
string frame_id //以激光扫描为例,它将是激光数据所在帧(坐标系)

float32 angle_min # scan的开始角度 [弧度]
float32 angle_max # scan的结束角度 [弧度]
float32 angle_increment # 测量的角度间的距离 [弧度]
float32 time_increment # 测量间的时间 [秒]
float32 scan_time # 扫描间的时间 [秒]
float32 range_min # 最小的测量距离 [米]
float32 range_max # 最大的测量距离 [米]
float32[] ranges # 测量的距离数据 [米] (注意: 值 < range_min 或 > range_max 应当被丢弃)
float32[] intensities # 强度数据 [device-specific units]

ros发布LaserScan消息(Python)

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
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
#!/usr/bin/env python
#-*- coding:utf-8 -*-

import math
import rospy
import airsim
import numpy as np
from sensor_msgs.msg import LaserSca

# 获取无人机的xyz坐标,为后面计算距离做准备
def get_drone_position(client):
position = client.getMultirotorState().kinematics_estimated.position
return position

# 将点云数据转换成相应角度和距离
def point_cloud_to_angle_position(pos,points):
obs_distance = []
angles = []
for i in range(len(points)):
x = round(points[i][0],2)
y = round(points[i][1],2)
z = round(points[i][2],2)
if x != 0:
angle = math.atan(y/x) * 180 / 3.14 # 利用三角函数关系求当前角度
angle = math.floor(angle) #向下取整
angles.append(angle)
distance = math.sqrt((pos.x_val -x) **2 + (pos.y_val-y) **2 +(pos.z_val - z)**2) # 根据激光点坐标和无人机当前点坐标求解距离
obs_distance.append(distance)
#print([i,angle,distance])
angles,obs_distance = scale_point_cloud(angles,obs_distance) # 进行相应变换
return angles,obs_distance

# 在180度范围内,每隔1度,取一个值,即将会取181个值(中间有0度)
# 对每个角度,求出其对应的下标有哪些,然后求均值,表示当前角度的激光点距离
def scale_point_cloud(angles,obs_distance):
angle_min = -90.0
angle_max = 90.0
new_angles = []
new_obs_distance = []
# address_index = [x for x in range(len(list_position_name)) if list_position_name[x] == i]
for i in range(int(angle_max - angle_min + 1)):
address_index = [x for x in range(len(angles)) if angles[x] == angle_min + i ] # 求每个角度的下标
if len(address_index) == 0: #如果某个角度没有值,则直接给最大值
distance = 100.0
else: # 否则,求均值
total_dis = 0
for j in range(len(address_index)):
total_dis += obs_distance[address_index[j]]
distance = total_dis / len(address_index)
new_angles.append(angle_min + i)
new_obs_distance.append(distance)
#print(new_angles[i],new_obs_distance[i])
return new_angles,new_obs_distance

# 发布ros数据
def pub_laserscan(obs_distance):
laserscan = LaserScan()
laserscan.header.stamp = rospy.Time.now()
laserscan.header.frame_id = 'lidar'
laserscan.angle_min = -1.57
laserscan.angle_max = 1.57 # 对应180度
laserscan.angle_increment = 3.14 / 180 #弧度的增量,这样就是隔1度取值
laserscan.time_increment = 1.0 / 10 / 180 # 中间的10对应于json中的RotationsPerSecond
laserscan.range_min = 0.0
laserscan.range_max = 100.0
laserscan.ranges = [] # 距离
laserscan.intensities = [] # 强度
for i in range(1,len(obs_distance)):
laserscan.ranges.append(obs_distance[i])
laserscan.intensities.append(0.0)
print(laserscan)
return laserscan

def main():

# connect the simulator
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)

scan_pub = rospy.Publisher('/scan', LaserScan, queue_size=10)
rate = rospy.Rate(1.0)

while not rospy.is_shutdown():

# get the lidar data
lidarData = client.getLidarData()
#print('lidar',lidarData)

if len(lidarData.point_cloud) >3:

points = np.array(lidarData.point_cloud,dtype=np.dtype('f4'))
points = np.reshape(points,(int(points.shape[0]/3),3))
#print('points:',points)
pos = get_drone_position(client)
angles,obs_distance = point_cloud_to_angle_position(pos,points)
print('number of points'),len(points)
laserscan = pub_laserscan(obs_distance)
scan_pub.publish(laserscan)
rate.sleep()
else:
print("\tNo points received from Lidar data")

if __name__ == "__main__":
rospy.init_node('drone1_lidar',anonymous=True)
main()

rviz显示LaserScan数据

要注意,在rviz界面中,将Style的类型设置为Points,否则可能会看不到点,具体的见PointCloud中的图片

2.3 Python代码编写(PointCloud)

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
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
#!/usr/bin/env python
#-*- coding:utf-8 -*-

import math
import rospy
import airsim
import numpy as np
from geometry_msgs.msg import Point32
from sensor_msgs.msg import LaserScan,PointCloud

def pub_pointcloud(points):
pc = PointCloud()
pc.header.stamp = rospy.Time.now()
pc.header.frame_id = 'lidar'

for i in range(len(points)):
pc.points.append(Point32(points[i][0],points[i][1],points[i][2]))
print('pc:',pc)
return pc

def main():

# connect the simulator
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)

pointcloud_pub = rospy.Publisher('/pointcloud', PointCloud, queue_size=10)
rate = rospy.Rate(1.0)

while not rospy.is_shutdown():

# get the lidar data
lidarData = client.getLidarData()
#print('lidar',lidarData)

if len(lidarData.point_cloud) >3:

points = np.array(lidarData.point_cloud,dtype=np.dtype('f4'))
points = np.reshape(points,(int(points.shape[0]/3),3))
#print('points:',points)
pc = pub_pointcloud(points)
pointcloud_pub.publish(pc)
rate.sleep()
else:
print("\tNo points received from Lidar data")

if __name__ == "__main__":
rospy.init_node('drone1_lidar',anonymous=True)
main()

rviz显示PointCloud数据

其中的Style要设置为Points

2.4 rviz同时显示LaserScan和PointCloud数据

上图中,红色的为LaserScan数据,白色的为PointCloud数据,能够从图中看出,红色的和白色的点还是很接近的。为什么不一致呢?因为LaserScan是封装成了180个点,并且对相同的角度的距离求均值得出来的,而PointCloud直接获取的是points数据,没有做任何的修改。因此PointCloud数据更真实一点,但LaserScan目前来说也还行。

三、多线激光雷达设置

3.1 settings.json设置

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
32
{
"SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
"SettingsVersion": 1.2,
"SimMode":"Multirotor",

"Vehicles":{
"Drone1":{
"VehicleType":"SimpleFlight",
"X":0,"Y":0,"Z":0,
"Roll": 0, "Pitch": 0, "Yaw" : 0,
"Sensors":
{
"MyLidar1":
{
"SensorType":6,
"Enabled":true,
"NumberOfChannels":16, # 设置16线激光
"PointsPerSecond":100000, # 这里将点的个数增加了,因为环境变得更复杂了一点
"X":0,"Y":0,"Z":-0.5,
"DrawDebugPoints":true,
"Roll": 0, "Pitch": 0, "Yaw" : 0,
"VerticalFOVUpper": -15,
"VerticalFOVLower": 25, # 垂直设置40度范围
"HorizontalFOVStart": -180,
"HorizontalFOVEnd": 180, # 水平设置360度
"DrawDebugPoints": true,
"DataFrame": "SensorLocalFrame"
}
}
}
}
}

测试环境(16线的显示)

3.2 PointCloud显示

代码和2.3节的代码一样

从上图中可以看出,由于channel修改为16,则会有圆圈产生,并且设置了垂直角度,因此,整个点云图看起来是3维的。

(1)若将垂直角度还是设置为0,则其点云图将会是二维显示

在json中,重新将垂直角度设为0,

1
2
"VerticalFOVUpper": 0,
"VerticalFOVLower": 0, # 重新设置为0度

则其点云图显示如下,则将会二维显示

(2)若垂直角度还是40度,增大channel,如增大为32或64时,显示效果如下:

设置为32线时,其环境中可视化的激光圈数明显增加

其rviz图为:

设置为64线时,环境中激光可视化

其rviz图:

从这个中可以看出,随着channel的增加,其环境中可视化的激光圈数明显增加,并且rviz里面的探测距离明显变得更远。

(3)垂直角度40度,64channel,并设置50w个点

尝试过设置100w个点,但是在启动sh文件时,UE4左上角会提示Lidar capping number of points to scan信息,感觉还是有些问题的,但是100w个点,仍然能显示。因此后面修改为50w个点,没有该信息提示

rviz显示图为:

和上一张图比较,增加更多的点,其扫描出的物体能够更精确。

rviz显示PointCloud并上色

在Color Transformer一栏,设置为AxisColor,则其效果见下图

四、比较

4.1 单线,50w点,无垂直角度,水平角度范围180

PointCloud显示

LaserScan显示

LaserScan和PointCloud同时显示

4.2 单线,50w点,垂直角度40度,水平角度范围180

红色的LaserScan数据,白色的为PointCloud数据,看起来很接近,但是添加了垂直角度后,单线的激光检测就不对了

4.3 多线(16),50w点,垂直角度40度,水平角度范围180

红色的为LaserScan,其他颜色的为PointCloud。

总结

通过从比较中可以看出,

(1)单线激光雷达,不适合设置垂直角度,只适合二维。就连PointCloud在单线垂直角度下,都显示不对

(2)多线激光雷达,一般设置垂直角度,显示三维点云数据

我目前设置的LaserScan,是对数据进行了处理,但是目前只考虑了180度的范围,没有考虑360度,因为360度中角度会发生相应的变化,后面会在继续进行处理~

参考链接

已经测试过的LaserScan转PointCloud2

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
32
33
34
35
36
37
38
39
40
#!/usr/bin/env python
#-*- coding:utf-8 -*-

import sensor_msgs.point_cloud2 as pc2
import rospy
from sensor_msgs.msg import PointCloud2, LaserScan
import laser_geometry.laser_geometry as lg
import math

rospy.init_node("laserscan_to_pointcloud")

lp = lg.LaserProjection()
pc_pub = rospy.Publisher("test", PointCloud2, queue_size=1)

def scan_cb(msg):
# convert the message of type LaserScan to a PointCloud2
pc2_msg = lp.projectLaser(msg)

pc_pub.publish(pc2_msg)

# convert it to a generator of the individual points
point_generator = pc2.read_points(pc2_msg)

# we can access a generator in a loop
sum = 0.0
num = 0
for point in point_generator:
if not math.isnan(point[2]):
sum += point[2]
num += 1
# we can calculate the average z value for example
print(str(sum/num))

# or a list of the individual points which is less efficient
point_list = pc2.read_points_list(pc2_msg)

print(point_list[len(point_list)/2].x)

rospy.Subscriber("/scan", LaserScan, scan_cb, queue_size=1)
rospy.spin()
0%