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

为了实现多机激光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

官方教程

1. 配置仓库，即勾选"restricted," “universe,” and “multiverse.”
   

2. 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'

3. Set up your keys

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

4. Installation

sudo apt update
sudo apt install ros-noetic-desktop

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

3 安装cartographer

官方教程

1. 安装依赖

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

2. 下载源码：创建一个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

3. 安装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超时问题的新方法

4. 安装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 类错误解决方法

5. 编译

catkin_make_isolated --install --use-ninja

6. 测试

# 下载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

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

4 详细配置cartographer

1. 配置文件

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

2. 输入配置
   所属文件：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.,
  },

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

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

扫描匹配有2种方式：

1. CeresScanMatcher匹配
   使用ceres优化库构建优化方程来进行匹配，这种匹配适用于提供良好位姿初始值的情况，可以实现亚像素的匹配，速度快，缺点是如果初始值不理想，匹配也有可能不理想。
2. RealTimeCorrelativeScanMatcher匹配（CSM匹配）
   当你没有其他数据或者不信任其他数据时，它使用了一种与回环检测相似的匹配策略，并把匹配的结果作为CeresScanMatcher匹配的初始值，这种匹配的代价较高它会淹没掉其他传感器的作用，但是它在特征丰富的环境比较鲁棒，缺点计算量增加。

关于子图

1. 一个子图内激光帧数是人为设定的，当一个子图接收到给定数量的激光数据后，子图被认为是完整的。
2. 一个子图内激光的数量不能太大，太大可能会产生位姿漂移（子图内部漂移要低于分辨率，即局部正确），也不能太小，太小不利于后台的回环检测。
3. 一般不把所有匹配的激光都插入子图，否则会造成冗余。
4. 占据概率，根据传感器配置

--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种是TSDF
      resolution = 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.5
        miss_probability = 0.49,				--传感器给出miss后，可信度，正常应该小于0.5
      },
    },
  },

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

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

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

1. 非全局约束，是建立与自身路径节点比较近时的约束，一般是与前一个子图和当前子图之间建立，目的是保持局部路径的连贯性。
2. 全局约束，是当检测到有回环，建立起的约束，一般是新生成的子图与之前的路径节点之间建立，目的是保持全局路径的连贯性。
3. 为了限制约束的数量，cartographer限制一部分的路径节点建立约束，并通过一个约束采样因子sampling_ratio来控制。
4. 太小的因子可能造成缺失约束和无效的回环检测，太大因子会降低全局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 。

1. 下载源码

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

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

# 编译
	catkin_make

# 连接雷达，并赋予设备权限（根据实际情况，选择设备名称）
	sudo chmod 777 /dev/ttyUSB0

# 跑个demo（记得source rplidar_ros的安装位置）
	roslaunch rplidar_ros test_rplidar.launch
	roslaunch 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

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

1. 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>

2. 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_pose
  use_pose_extrapolator = true,
  use_odometry = false,							--是否订阅里程计话题，nav_msgs/Odometry
  use_nav_sat = false,							--是否订阅 sensor_msgs/NavSatFix
  use_landmarks = false,						--是否订阅 cartographer_ros_msgs/LandmarkList 
  
  num_laser_scans = 1,							--订阅激光扫描话题数量，sensor_msgs/LaserScan
  num_multi_echo_laser_scans = 0,				--订阅多回波激光扫描话题数量，sensor_msgs/MultiEchoLaserScan
  num_subdivisions_per_laser_scan = 1,			--将每个接收到的（多回波）激光扫描分成的点云数
  num_point_clouds = 0,							--订阅点云话题数量，sensor_msgs/PointCloud2
  lookup_transform_timeout_sec = 0.2,			--使用tf2查找变换的超时秒数
  submap_publish_period_sec = 0.3,				--发布子图间隔
  pose_publish_period_sec = 5e-3,				--发布位姿间隔，5e-3 即 0.005s，即200Hz
  trajectory_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

3. 关于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

# 首先运行 cartographer
	roslaunch cartographer_ros cartoForRplidar.launch

# 然后运行 rplidar_ros
	roslaunch 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 使用与测试

1. 所有话题桥接

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

2. 具体桥接话题情况
   仅桥接基本类型的话题，对于自定义消息类型（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.

3. 部分话题桥接
   1. 编写 bridge.yaml 文件如下

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

   2. 加载参数

   rosparam load bridge.yaml
   

   3. 启动节点后，运行参数桥

   ros2 run ros1_bridge parameter_bridge
   

9 镜像备份与恢复

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

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

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

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

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

最后

以上就是缓慢滑板为你收集整理的ROS2_Foxy学习10——多机激光SLAM准备篇的全部内容，希望文章能够帮你解决ROS2_Foxy学习10——多机激光SLAM准备篇所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。