硬件环境
RaspBerry PI 4B
rplidar A2 M12

软件环境
Ubuntu Server 20.04 LTS (respberry)
Ubuntu Server 20.04 LTS (工程机)
ros galactic

1. 连接雷达

1.1 连接

思岚雷达标配一个雷达、一个转接器、一根电源线、一根数据线,首先将这四部分全部连接好,电源线的USB type-A端需要接5V电源,这个需要自己准备,数据线的USB type-A端直接连接树莓派。

如此连接后,在树莓派执行命令:

1
ls /dev/ttyUSB*

如果你的树莓派USB接口只连接了雷达的话,应该会出现ttyUSB0

1.2 使用 udev 将雷达绑定为静态设备

注:这一部分不是必须的。

首先使用lsusb命令,获取雷达的pidvid

pi@raspberrypi:~/project/slam$ lsusb
Bus 003 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub
Bus 002 Device 002: ID 1058:25a1 Western Digital Technologies, Inc. Elements / My Passport (WD20NMVW)
Bus 002 Device 001: ID 1d6b:0003 Linux Foundation 3.0 root hub
Bus 001 Device 003: ID 10c4:ea60 Silicon Labs CP210x UART Bridge
Bus 001 Device 002: ID 2109:3431 VIA Labs, Inc. Hub
Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub

获取到如上信息,那么vid10c4pidea60,如果不知道你的雷达是上面的哪一个设备,可以插拔USB,对比插拔前后的输出,判断出是哪一个设备。

编辑udevrules

1
vim /etc/udev/rule.d/smartcar.rules

此处,smartcar处的文件名可以随意取名,但是其后缀必须是.rules

输入内容:

1
KERNEL=="ttyUSB?", SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE="0777" SYMLINK+="rplidar"

idVendor处填写vididProduct处填写pid

rplidar处的名称也可以随意取名。

保存,执行:

1
sudo service udev restart

重新插拔设备,如果准确无误,执行ll /dev/rplidar将看到:

1
/dev/rplidar -> ttyUSB0

2. 安装 ros galactic

此处和《Windows 11 WSL2 安装并运行 ROS》一文的2.2节相同,不再赘述。

3. 安装 gmapping

进入你的ROS2工程的src目录,执行:

1
2
3
4
git clone https://github.com/Project-MANAS/slam_gmapping
cd ../
colcon build
source ./install/setup.sh

4. 运行雷达

4.1 运行雷达节点

雷达节点的代码段为:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
lidar_node = Node(
name='sllidar_node',
package='sllidar_ros2',
executable='sllidar_node',
output='screen',
parameters=[{
'serial_port': '/dev/rplidar',
'serial_baudrate': '256000',
'frame_id': 'laser',
'inverted': False,
'angle_compensate': True,
'scan_mode': 'Sensitivity',
}],
)

serial_port/dev中的设备,此处若未设置udev rules,应写为ttyUSB0(以实际情况为准);serial_baudrate即波特率,A1A2一般为115200A3256000,但A2 M7A2 M12比较特殊,是256000,此处需要准确,如果不知道自己的设备波特率是多少,建议去思岚官网查询,需要注意的是,不仅此处的值要与设备对应,设备连接的转接盒上也有一个可以拨动的开关,也要与这个值相同。

运行节点:

1
2
3
4
5
6
# Launch
return LaunchDescription([
lidar_node,
tf1,
tf2,
])

需要注意的是,此处还应设置两个tf

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
tf1 = Node(
name='tf1',
package='tf2_ros',
executable='static_transform_publisher',
arguments = ['0', '0', '0', '0', '0', '0', 'odom', 'laser'],
output='screen',
)

tf2 = Node(
name='tf2',
package='tf2_ros',
executable='static_transform_publisher',
arguments = ['0', '0', '0', '0', '0', '0', 'odom', 'base_link'],
output='screen',
)

否则会因找不到参考坐标系,报出如下错误:

[slam_gmapping-2] [INFO] [1660630271.191481990] [slam_gmapping]: Message Filter dropping message: frame 'laser' at time 1660630270.044 for reason 'discarding message because the queue is full'

Warning: Invalid frame ID "base_link" passed to canTransform argument target_frame - frame does not exist

4.2 雷达数据(LaserScan)解析

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
sensor_msgs.msg.LaserScan(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1667629747, nanosec=468238273), frame_id='laser'), 
# 测量的最小角度
angle_min=-3.1241390705108643,
# 测量的最大角度
angle_max=3.1415927410125732,
# 扫描的角度间隔
angle_increment=0.0034828970674425364,
# 扫描的时间间隔
time_increment=5.000018063583411e-05,
# 测量的时间间隔
scan_time=0.0899503231048584,
# 测量的最小距离
range_min=0.15000000596046448,
# 测量的最大距离
range_max=16.0,
# 测量的数据
ranges=[0.25, 0.25099998712539673, 0.25200000405311584, 0.25200000405311584, 0.25200000405311584, inf, ...],
# 测量强度
intensities=[47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, 47.0, ...])

雷达完成一次完整的测量其数据如上,其数据可以通过ros2 topic echo /scan查看,但若是想查看完整的数据,还是要自己写一个订阅者,并把数据打印出来。

雷达节点会源源不断地在/scan话题中发布如上数据,此处将一次完整的测量数据称为一帧数据,帧与帧之间可以依靠其话题头部区分:

1
2
# 帧头
sensor_msgs.msg.LaserScan(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1667629747, nanosec=468238273), frame_id='laser'),

头部中包含以下信息:sec表示本帧开始时间的秒部分,nanosec表示本帧开始时间的纳秒部分,frame_id表示数据的参考系。
对于话题头部之后的测量数据,

其中angle_minangle_max应该很好理解,可以看到这两个值,一个接近,一个接近,实际上就是雷达从开始扫描,一直扫描到处,即为,完成完整的一圈测量;angle_increment即每两个相邻的扫描间隔的角度,这里用2π / angle_increment,会发现这个值相当接近于 1800,而rangesintensities这里我虽然用省略号省略了数据,其实这两个数组的长度也正好是 1800,显然,rangesintensities每一个元素对应的值就是每次扫描采集的数据,而完整的一圈测量一共扫描了 1800 次;time_increment也无需多言,即每两个相邻的扫描之间间隔的时间;scan_time是完成一圈完整的测量所话费的总时长,那么显然scan_time / time_increment的值也接近 1800;range_minrange_max是每次扫描的有效区间,低于或超出这个区间的值都应舍去;ranges是扫描到雷达的距离,因为range_minrange_max的存在,ranges数组中常会看到某个元素是inf,即infinity,无穷小或无穷大,其实就是无效数据;intensities是每次扫描反射的激光束强度,和设备、激光扫描到的物体材质有关,但这个数据我们一半不太用到,因为建图不需要。

4.3 绘制雷达扫描数据

那么如何绘制雷达扫描出来的数据呢?

雷达以自身为圆心旋转,每隔相同的角度,不断采集物体到自身的距离。那么很显然,要想绘制出雷达感知到的环境,很显然是利用极坐标:其极角是第n次旋转的角度n * angle_increment,即极径即ranges[n-1],那么这样根据一次完整的测量数据,很容易绘制出下图:

图 4-1 扫描数据绘图

这里放出绘图代码:

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
# 一次扫描的数据
arr_str = [...]

import numpy as np
import matplotlib.pyplot as plt

# total 1800
__LEN__ = 1800
# 绘制出的数据点大小
__POINT_SIZE__ = 5

# 转为数组
arr = arr_str.split(',')

# 极角,一共 1800 个数据
theta = np.arange(0,2 * np.pi, 2 * np.pi/__LEN__)
# 这个可以不要,这样直接赋值给 colors,每个点的颜色会自动和数据大小对应,颜色要和角度对应,colors = theta 就可以了
colors = arr

# 转 float
for i in range(0, len(arr)):
arr[i] = float(arr[i])
inten[i] = float(inten[i])

ax2 = plt.subplot(111, projection='polar')
ax2.scatter(theta, arr, s=__POINT_SIZE__, c=colors, cmap='hsv', alpha =0.75)
# 去掉每条数据的标签,否则图会很难看
ax2.set_rticks([])

# 绘图
plt.show()

5. 运行 gmapping

gmapping的代码段为:

1
2
3
4
5
6
gmapping = Node(
package='slam_gmapping',
executable='slam_gmapping',
output='screen',
parameters=[{'use_sim_time':use_sim_time}]
)

4.1节相同,应在LaunchDescription的数组参数中添加gmapping节点。

6. Rviz查看雷达数据和gmapping建图

在工程机运行rviz2:

1
rviz2

rviz2中添加topic,点击rviz2软件界面左下角的“add”按钮,在窗口中选择“By topic”,局域网中设备发布的ros话题将会全部在此处显示出来。

选择LaserScanMap,即订阅 /scan/map相关话题,如下图:

图 6-1 rviz2 中添加 topic

添加话题后,将Rviz2的左侧边栏的Fixed Frame选择为map,其意为将map帧作为固定帧,其余帧相对map帧运动。据此可知,若选择base_link为固定帧,即小车的帧固定,其余帧相对运动。此处可类比地图软件的“地图固定”和“跟随视角”方便理解。

Rviz2展示数据如下图:

图 6-2 rviz2 展示数据

gmapping节点可以正常运行后,能够发现此处存在地图漂移的问题:

图 6-3 地图漂移

因为odom里程计话题没有正确计算和发布,地图扫描算法需要依据odom坐标及话题建图,本文下节将进行odom的计算和话题发布。

7. odom 计算及话题发布

待编辑

8. 可能出现的问题

待编辑

9. 参考

  1. What are LaserScan intensities?