在树莓派上连接思岚雷达并运行 slam 算法

硬件环境
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命令，获取雷达的pid和vid：

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

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

编辑udev的rules：

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处填写vid，idProduct处填写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目录，执行：

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

4. 运行雷达

4.1 运行雷达节点

雷达节点的代码段为：

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即波特率，A1、A2一般为115200，A3为256000，但A2 M7与A2 M12比较特殊，是256000，此处需要准确，如果不知道自己的设备波特率是多少，建议去思岚官网查询，需要注意的是，不仅此处的值要与设备对应，设备连接的转接盒上也有一个可以拨动的开关，也要与这个值相同。

运行节点：

# Launch
return LaunchDescription([
    lidar_node,
    tf1,
    tf2,
])

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

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）解析

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

4.3 绘制雷达扫描数据

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

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

图 4-1 扫描数据绘图

这里放出绘图代码：

# 一次扫描的数据
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的代码段为：

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：

rviz2

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

选择LaserScan和Map，即订阅 /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. 参考

What are LaserScan intensities?