ROS2_Foxy学习10——多机激光SLAM准备篇

1 安装Ubuntu20.04 mate
2 安装ROS noetic
3 安装cartographer
4 详细配置cartographer
5 安装RPLidar A1
6 测试建图
- 6.1 配置cartographer
- 6.2 配置RPLidar A1
- 6.3 运行测试
7 安装ROS foxy
8 安装ros1_bridge
- 8.1 安装
- 8.2 使用与测试
9 镜像备份与恢复

为了实现多机激光SLAM，在上一篇文章《ROS2_Foxy学习9——多机激光SLAM先导篇》中讨论了开源激光SLAM（Cartographer）在ROS2.0中运行的可行性。最后得出的小结是各开源激光SLAM算法对ROS2.0的适配还在进行中（2021年初），暂无法在ROS2.0上运行，因此决定曲线救国，在ROS1.0上跑SLAM，然后通过ros1_bridge这个功能包将产生的地图、车辆位姿、点云等信息上传到DDS，然后由PC端的ROS2程序处理（地图合并等工作）。基于上述情况，在车载的树莓派上安装了两个ROS（16G的SD卡是搞不定的，最少32G）。

odk，下面介绍在树莓派上部署Ubuntu 20.04mate、ROS noetic、ROS foxy、cartographer、rplidar等的过程，由于内容较多，有些部分会直接给出之前写过的一些博文而不再重复记录。

下一篇博文将介绍地图融合相关的内容。

1 安装Ubuntu20.04 mate

参考：树莓派4B——Ubuntu20.04 mate
包括：系统ubuntu20.04 mate，wiringpi库，网络与串口工具，远程，开机启动等。

2 安装ROS noetic

官方教程

配置仓库，即勾选"restricted," “universe,” and “multiverse.”
Setup your sources.list

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

Set up your keys

sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

Installation

sudo apt update
sudo apt install ros-noetic-desktop

有不同的桌面版本，基于树莓派选择 ros-noetic-desktop

3 安装cartographer

官方教程

安装依赖

sudo apt-get update
sudo apt-get install -y python3-wstool python3-rosdep ninja-build stow

下载源码：创建一个ros工作空间，然后下载源码

mkdir catkin_ws
cd catkin_ws
wstool init src
wstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstall
wstool update -t src

关于github解析错误的问题，raw.githubusercontent.com 拒绝连接

到https://www.ipaddress.com查一下raw.githubusercontent.com的ipv4地址
修改 /etc/hosts文件加上下面这句命令
ipv4地址 raw.githubusercontent.com

安装cartographer_ros依赖

sudo rosdep init
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y

The command ‘sudo rosdep init’ will print an error if you have already executed it since installing ROS. This error can be ignored.

解决ROS系统 rosdep update超时问题的新方法

安装abseil-cpp library

src/cartographer/scripts/install_abseil.sh
# Due to conflicting versions you might need to uninstall the ROS abseil-cpp using
sudo apt-get remove ros-${ROS_DISTRO}-abseil-cpp

git clone 出现fatal: unable to access 'https://github 类错误解决方法

编译

catkin_make_isolated --install --use-ninja

测试

# 下载bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag
# 跑
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag

问题
安装cartographer，找不到 “absl” 解决方法

4 详细配置cartographer

配置文件

# local slam部分 2D配置文件
cartographer/configuration_files/trajectory_builder_2d.lua
# global slam部分 配置文件
cartographer/configuration_files/pose_graph.lua

输入配置
所属文件：cartographer/configuration_files/trajectory_builder_2d.lua

统一: 距离单位是米，时间单位是秒

--输入配置use_imu_data = true,              --imu使用与否min_range = 0.,                   --过滤 水平距离范围 下限max_range = 30.,                 --过滤 水平距离范围 上限min_z = -0.8,                        max_z = 2.,missing_data_ray_length = 5.,      --对于超过最大范围的点，用missing_data_ray_length代替num_accumulated_range_data = 1,  --数据源数量voxel_filter_size = 0.025,      --体素网格化下采样 体素大小   解决点云近密远稀的问题 --【实时】 【越大 计算量越小 丢失点越多】adaptive_voxel_filter = {         --体素网格化下采样 自适应max_length = 0.5,                    --采样时间间隔min_num_points = 200,              --max_range = 50.,},loop_closure_adaptive_voxel_filter = {            --体素网格化下采样 回环自适应max_length = 0.9,min_num_points = 100,max_range = 50.,},

Local SLAM配置
所属文件：cartographer/configuration_files/trajectory_builder_2d.lua（已删除TSDF部分）

Local SLAM 通过位姿推断器（pose extrapolator）来给予每一帧激光匹配的初始位姿，将每一帧激光与当前子图进行扫描匹配，匹配后将激光插入子图。

扫描匹配有2种方式：

CeresScanMatcher匹配
使用ceres优化库构建优化方程来进行匹配，这种匹配适用于提供良好位姿初始值的情况，可以实现亚像素的匹配，速度快，缺点是如果初始值不理想，匹配也有可能不理想。

RealTimeCorrelativeScanMatcher匹配（CSM匹配）
当你没有其他数据或者不信任其他数据时，它使用了一种与回环检测相似的匹配策略，并把匹配的结果作为CeresScanMatcher匹配的初始值，这种匹配的代价较高它会淹没掉其他传感器的作用，但是它在特征丰富的环境比较鲁棒，缺点计算量增加。

关于子图

一个子图内激光帧数是人为设定的，当一个子图接收到给定数量的激光数据后，子图被认为是完整的。

一个子图内激光的数量不能太大，太大可能会产生位姿漂移（子图内部漂移要低于分辨率，即局部正确），也不能太小，太小不利于后台的回环检测。

一般不把所有匹配的激光都插入子图，否则会造成冗余。

占据概率，根据传感器配置

--Local SLAM配置--【实】没有imu，可能需要打开这个，同时前端会比较消耗资源，原因见上use_online_correlative_scan_matching = false,        real_time_correlative_scan_matcher = {linear_search_window = 0.1,                     --最大搜索距离angular_search_window = math.rad(20.),         --最大搜索角度translation_delta_cost_weight = 1e-1,          --平移权值rotation_delta_cost_weight = 1e-1,               --旋转权值，如果机器人不常旋转，则改权值可以适当提高},--【实】对输入数据赋予权重，这个权重是对测量数据的信任度，可以被看作静态的协方差。权重越大，cartographer将越偏向这种参数规则。ceres_scan_matcher = {occupied_space_weight = 1.,translation_weight = 10.,rotation_weight = 40.,ceres_solver_options = {use_nonmonotonic_steps = false,max_num_iterations = 20,                        --提高收敛速度 迭代次数num_threads = 1,                              --提高收敛速度 线程数},},--【实】减少子图冗余数据 两帧之间，当运动超过时间、距离、角度阈值后，才将扫描插入子图motion_filter = {max_time_seconds = 5.,max_distance_meters = 0.2,max_angle_radians = math.rad(1.),},--子图配置submaps = {num_range_data = 90,                       --一个子图内最大点云帧数grid_options_2d = {grid_type = "PROBABILITY_GRID",         --存储格式 1种是概率图，1种是TSDFresolution = 0.05,                     --【实】分辨率},range_data_inserter = {range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",probability_grid_range_data_inserter = {  --概率栅格地图insert_free_space = true,hit_probability = 0.55,                    --传感器给出hit后，可信度，正常应该大于0.5miss_probability = 0.49,                --传感器给出miss后，可信度，正常应该小于0.5},},},

Global SLAM配置
所属文件：cartographer/configuration_files/pose_graph.lua (已删除3d部分）

Global SLAM又可以被称作图优化，通过建立路径节点和子图之间的约束，然后优化这些约束
路径节点和子图是空间位姿，约束是空间位姿的旋转平移矩阵，用ceres的自动求导省去了自己求雅可比的工作。

约束分为2种：非全局约束和全局约束

非全局约束，是建立与自身路径节点比较近时的约束，一般是与前一个子图和当前子图之间建立，目的是保持局部路径的连贯性。

全局约束，是当检测到有回环，建立起的约束，一般是新生成的子图与之前的路径节点之间建立，目的是保持全局路径的连贯性。

为了限制约束的数量，cartographer限制一部分的路径节点建立约束，并通过一个约束采样因子sampling_ratio来控制。

太小的因子可能造成缺失约束和无效的回环检测，太大因子会降低全局SLAM的效率而且无法进行实时回环。

分支定界
类似于模板匹配使用图像金字塔的策略，先在低分辨率上进行搜索，然后逐渐在高分辨率上搜索。
It works on an exploration tree whose depth can be controlled.

POSE_GRAPH = {optimize_every_n_nodes = 90,                 --【实】每n次插入，优化一次，n==0时，即关闭全局优化constraint_builder = {sampling_ratio = 0.3,                       --【实】约束采样因子max_constraint_distance = 15.,              --min_score = 0.55,                            --大于最小匹配值 则FastCorrelativeScanMatcher找到了足够好的匹配global_localization_min_score = 0.6,loop_closure_translation_weight = 1.1e4,    --残差项1/5    全局约束loop_closure_rotation_weight = 1e5,            --残差项2/5    全局约束log_matches = true,                            --约束的直方图形式的日志报告--实时回环fast_correlative_scan_matcher = {         linear_search_window = 7.,             --最大搜索距离angular_search_window = math.rad(30.), --最大搜索角度branch_and_bound_depth = 7,                --分支定界 深度可控的搜索树 递归深度},--一但FastCorrelativeScanMatcher找到了足够好的匹配（大于最小匹配值），再用Ceres Scan Matcher进一步优化位姿（匹配结果）ceres_scan_matcher = {                      occupied_space_weight = 20.,translation_weight = 10.,rotation_weight = 1.,ceres_solver_options = {use_nonmonotonic_steps = true,max_num_iterations = 10,num_threads = 1,},},},matcher_translation_weight = 5e2,             --残差项3/5    局部约束matcher_rotation_weight = 1.6e3,               --残差项4/5    局部约束optimization_problem = {                       --残差项5/5    其他约束huber_scale = 1e1,                         --控制错误值影响大小 Huber因子越大，错误值对整体的影响越大acceleration_weight = 1.1e2,rotation_weight = 1.6e4,local_slam_pose_translation_weight = 1e5,local_slam_pose_rotation_weight = 1e5,odometry_translation_weight = 1e5,odometry_rotation_weight = 1e5,fixed_frame_pose_translation_weight = 1e1,fixed_frame_pose_rotation_weight = 1e2,fixed_frame_pose_use_tolerant_loss = false,fixed_frame_pose_tolerant_loss_param_a = 1,fixed_frame_pose_tolerant_loss_param_b = 1,log_solver_summary = false,use_online_imu_extrinsics_in_3d = true,fix_z_in_3d = false,ceres_solver_options = {use_nonmonotonic_steps = false,max_num_iterations = 50,num_threads = 7,},},--一旦轨迹完成，cartographer将执行新的全局优化，并且通常比以前的全局优化进行更多的迭代--这样做是为了完善cartographer的最终结果，通常不需要实时进行，因此大量迭代通常是正确的选择max_num_final_iterations = 200,             --最大迭代次数global_sampling_ratio = 0.003,log_residual_histograms = true,global_constraint_search_after_n_seconds = 10.,--  overlapping_submaps_trimmer_2d = {--    fresh_submaps_count = 1,--    min_covered_area = 2,--    min_added_submaps_count = 5,--  },
}

5 安装RPLidar A1

参考官方文档：https://github.com/Slamtec/rplidar_ros 。

下载源码

git clone https://github.com/Slamtec/rplidar_ros.git

在工作空间内编译，配置串口，然后跑demo

# 编译catkin_make# 连接雷达，并赋予设备权限（根据实际情况，选择设备名称）sudo chmod 777 /dev/ttyUSB0# 跑个demo（记得source rplidar_ros的安装位置）roslaunch rplidar_ros test_rplidar.launchroslaunch rplidar_ros view_rplidar.launch

6 测试建图

新建工作空间，并把cartographer和rplidar_ros的源码包放到/src文件夹下。

6.1 配置cartographer

编写launch文件和lua配置文件，直接参考cartographer_ros包下的两个文件:

cartographer_ros/launch/demo_revo_lds.launch
cartographer_ros/configuration_files/revo_lds.lua

改动的文件内容如下，这里是重新编写了两个文件，并将这两个新文件放在了跟参考文件同一个文件夹内：

launch文件：cartoForRplidar.launch <—— demo_revo_lds.launch

<launch>  <--！false 是因为要使用真实雷达数据，不用仿真时间 /--><param name="/use_sim_time" value="false" />  <--！注意configuration_basename 后面的配置文件名，要与实际的lua文件名一致 /--><node name="cartographer_node" pkg="cartographer_ros"  type="cartographer_node" args="  -configuration_directory $(find cartographer_ros)/configuration_files  -configuration_basename rplidar.lua"  output="log">  <--！将后面的scan修改成机器人发布的激光topic /--><remap from="scan" to="scan" />  </node>  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"type="cartographer_occupancy_grid_node" args="-resolution 0.05" /><node name="rviz" pkg="rviz" type="rviz" required="true"  args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>

lua文件：rplidar.lua <—— revo_lds.lua
这个文件只改了tracking_frame和published_frame，这两个frame，一般是laser，需要根据雷达ros驱动的实际情况配置。

include "map_builder.lua"
include "trajectory_builder.lua"options = {map_builder = MAP_BUILDER,trajectory_builder = TRAJECTORY_BUILDER,map_frame = "map",                         --cartographer使用的全局坐标系，最好保持默认，否则ROS的Rviz不认识其它的定义，导致无法可视化建图过程   --用来发布子图，是poses的父帧，通常是“map”--和odom最开始的时候是一个原点，但时间累计对产生累积误差tracking_frame = "base_link",                 --机器人中心坐标系，由SLAM算法跟踪的坐标系published_frame = "base_link",                --这个frame是用来正在发布poses的子帧，和map_frame对应--一般就将其设置为"base_link",这不就是"map->base_link"odom_frame = "odom",                            --published_frame 和 map_frame之间的框架，用于发布（非闭环）local SLAM结果provide_odom_frame = true,                    --  publish_frame_projected_to_2d = false,     --是否限制为纯2d位姿publish_tracked_pose = true,                   --发布tracked_poseuse_pose_extrapolator = true,use_odometry = false,                            --是否订阅里程计话题，nav_msgs/Odometryuse_nav_sat = false,                           --是否订阅 sensor_msgs/NavSatFixuse_landmarks = false,                     --是否订阅 cartographer_ros_msgs/LandmarkList num_laser_scans = 1,                         --订阅激光扫描话题数量，sensor_msgs/LaserScannum_multi_echo_laser_scans = 0,               --订阅多回波激光扫描话题数量，sensor_msgs/MultiEchoLaserScannum_subdivisions_per_laser_scan = 1,          --将每个接收到的（多回波）激光扫描分成的点云数num_point_clouds = 0,                            --订阅点云话题数量，sensor_msgs/PointCloud2lookup_transform_timeout_sec = 0.2,           --使用tf2查找变换的超时秒数submap_publish_period_sec = 0.3,               --发布子图间隔pose_publish_period_sec = 5e-3,                --发布位姿间隔，5e-3 即 0.005s，即200Hztrajectory_publish_period_sec = 30e-3,      --发布轨迹标记时间间隔rangefinder_sampling_ratio = 1.,               --range消息采样频率odometry_sampling_ratio = 1.,                 --odo消息采样频率fixed_frame_pose_sampling_ratio = 1.,           --imu_sampling_ratio = 1.,                     --imu消息采样频率landmarks_sampling_ratio = 1.,              --
}MAP_BUILDER.use_trajectory_builder_2d = true  --使用2D--local slam 配置
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35      --一个子图内最大点云帧数
TRAJECTORY_BUILDER_2D.min_range = 0.3                  --水平范围内过滤点云 下限
TRAJECTORY_BUILDER_2D.max_range = 30.                  --水平范围内过滤点云 上限
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.     --范围外的设为1米
TRAJECTORY_BUILDER_2D.use_imu_data = false             --imu使用与否TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true                             --【实】没有imu，可能需要打开这个，同时前端会比较消耗资源，原因见上
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1                --最大搜索范围 距离
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.   --最大搜索范围 角度
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1--global slam 配置
POSE_GRAPH.optimization_problem.huber_scale = 1e2      --控制错误值影响大小 Huber因子越大，错误值对整体的影响越大
POSE_GRAPH.optimize_every_n_nodes = 35                 --【实】每n次插入，优化一次，n==0时，即关闭全局优化
POSE_GRAPH.constraint_builder.min_score = 0.65         --大于最小匹配值 则FastCorrelativeScanMatcher找到了足够好的匹配return options

关于lua配置的一些记录
参考https://google-cartographer-ros.readthedocs.io/en/latest/configuration.html

6.2 配置RPLidar A1

修改launch文件，添加tf转换，主要在test_rplidar上修改，如下

<launch><node pkg="tf2_ros" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 1 base_link laser" />   <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="log"><param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  <param name="serial_baudrate"     type="int"    value="115200"/><param name="frame_id"            type="string" value="laser"/><param name="inverted"            type="bool"   value="false"/><param name="angle_compensate"    type="bool"   value="true"/></node><node name="rplidarNodeClient"          pkg="rplidar_ros"  type="rplidarNodeClient" output="log"></node>
</launch>

6.3 运行测试

catkin_make_isolated# 首先运行 cartographerroslaunch cartographer_ros cartoForRplidar.launch# 然后运行 rplidar_rosroslaunch rplidar_ros test_rplidar.launch

7 安装ROS foxy

安装过程及问题，见这里。

8 安装ros1_bridge

8.1 安装

树莓派编译ros2时，没有编译ros1_bridge，ros1_bridge需要在ros1和ros2两个环境下单独编译，过程见官网，时间1小时。

ros1_bridge使用方法，见这里。

8.2 使用与测试

所有话题桥接

ros2 run ros1_bridge dynamic_bridge --bridge-all-topics      # 桥接所有话题，不加参数时，只有发布和订阅成对时，才桥接

具体桥接话题情况
仅桥接基本类型的话题，对于自定义消息类型（cartographer定义的消息类型也算）需要修改源码。
<1> 全局地图：cartographer发布的全局地图话题 /map 是 nav_msgs/OccupancyGrid 类型可以桥接。
<2> 位姿：TF可以桥接成功。计算位姿tracked_pose (geometry_msgs/PoseStamped)没有发布，Only published if the parameter publish_tracked_pose is set to true.

部分话题桥接

编写 bridge.yaml 文件如下

topics:-topic: /individual_map_r1  # ROS1 topic nametype: nav_msgs/msg/OccupancyGrid  # ROS2 type namequeue_size: 1  # For the publisher back to ROS1-topic: /tracked_pose_1  # ROS1 topic nametype: geometry_msgs/msg/PoseStamped  # ROS2 type namequeue_size: 1  # For the publisher back to ROS

加载参数

rosparam load bridge.yaml

启动节点后，运行参数桥

ros2 run ros1_bridge parameter_bridge

9 镜像备份与恢复

为避免重新配置的诸多问题，可以将配置好的系统进行镜像备份（复制），然后写入新卡即可。

将已配置好系统的SD卡插入读卡器，然后接入linux系统电脑，查看磁盘分区名，如图是/media的两个分区
镜像备份：使用dd命令将整个分区镜像，同时压缩镜像，这里终端不会有进度显示，只需要等，30G的卡大概30min

sudo dd if=/dev/sdc | gzip>自定义路径/压缩包名.gz

恢复镜像：将格式化的新卡（空间不能比原卡小）插入读卡器，应该有一个分区（如 /dev/sdc1），执行如下dd命令，将开始写卡，时间长于备份，30G约两个小时，但是搞完之后，启动分区识别不了，因此还是用树莓派的烧录软件，又快又稳。

sudo gzip -dc 自定义路径/压缩包名.gz | sudo dd of=/dev/sdc