ROS2_Foxy学习9——多机激光SLAM先导篇

1 环境准备
2 ROS1下测试SLAM
- 2.1 cartographer 源码测试
- 2.2 rplidar_ros 源码测试
- 2.3 rplidar_ros + cartographer
3 ROS2下测试SLAM
- 3.1 cartographer 安装测试（失败）
- 3.2 rplidar_ros 移植到 ROS2
4 ROS1 & ROS2下测试SLAM
- 4.1 ros1_bridge
- 4.2 ROS1 & ROS2 & cartographer
参考

这篇文章的整体目的是在多个车上跑slam，通过ROS2_Foxy将地图集中到一个电脑下，然后拼接成一张地图。
一开始的想法是车上用ROS2跑SLAM，由于各大开源SLAM算法对ROS2不支持或者支持不完善（又因为懒能力不够，不能把代码从ROS1移植到ROS2），因而，使用ROS1跑SLAM，然后通过ROS2的包ros1_bridge在各车与电脑之间传递数据，从而实现此项目。
在引入ros1_bridge之前，先写一写在ROS1上跑开源SLAM，以及在ROS2上跑开源SLAM的一些过程。
因为没做完内容较多，所以分了几个部分来写，这一篇是探索可行性的。

1 环境准备

设备	处理器	系统	ROS1版本	ROS2版本
电脑	amd64_酷睿i7 5500U_内存8GB	Ubuntu 20.04 LTS	Noetic	Foxy
车	arm64_树莓派4B_内存4GB	Ubuntu mate 20.04 LTS	-	Foxy

激光雷达使用思岚RPLidarA1，由于这个雷达频率较低，因此开源SLAM选择cartographer。没有选择Hector的原因除了RPLidarA1扫描频率太低，用Hector效果不理想外，Hector官方没有给ROS2的版本，自己移植太费时间。

2 ROS1下测试SLAM

2.1 cartographer 源码测试

参考官方文档：https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html 。

# 安装工具sudo apt-get updatesudo apt-get install -y python3-wstool python3-rosdep ninja-build stow# 下载源码mkdir catkin_wscd catkin_wswstool init srcwstool merge -t src https://raw.githubusercontent.com/cartographer-project/cartographer_ros/master/cartographer_ros.rosinstallwstool update -t src# 安装依赖sudo rosdep initrosdep updaterosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y# 手动安装abseil-cpp库src/cartographer/scripts/install_abseil.sh# 编译安装. /opt/ros/noetic/setup.bash  #（记得source ros1的安装位置）catkin_make_isolated --install --use-ninja# 跑个包. install_isolated/setup.bash     #（记得source cartographer的安装位置）wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bagroslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag

跑的bag大概是这样子：

2.2 rplidar_ros 源码测试

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

# 下载源码mkdir -p catkin_ws/srccd catkin_ws/srcgit clone https://github.com/Slamtec/rplidar_ros.git# 编译. /opt/ros/noetic/setup.bash #（记得source ros1的安装位置）catkin_make# 连接雷达，并赋予设备权限（根据实际情况，选择设备名称）sudo chmod 777 /dev/ttyUSB0# 跑个demo（记得source rplidar_ros的安装位置）. devel/setup.bash       # 因为只编译，没有安装，所以没有install文件夹roslaunch rplidar_ros test_rplidar.launch

跑的是这样子：
再跑个rviz的demo

 roslaunch rplidar_ros view_rplidar.launch

跑的是这样子：

2.3 rplidar_ros + cartographer

1、准备源码
新建工作空间，并把cartographer和rplidar_ros的源码包放到/src文件夹下。
2、编写cartographer的launch文件和lua配置文件
直接参考源码给的demo来改，主要参考了cartographer_ros包下的launch/demo_revo_lds.launch及其对应的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="screen">  <--！这句实际上，可以删掉 /--><remap from="scan" to="scan" />  </node>  <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",tracking_frame = "laser",published_frame = "laser",odom_frame = "odom",provide_odom_frame = true,publish_frame_projected_to_2d = false,use_pose_extrapolator = true,use_odometry = false,use_nav_sat = false,use_landmarks = false,num_laser_scans = 1,num_multi_echo_laser_scans = 0,num_subdivisions_per_laser_scan = 1,num_point_clouds = 0,lookup_transform_timeout_sec = 0.2,submap_publish_period_sec = 0.3,pose_publish_period_sec = 5e-3,trajectory_publish_period_sec = 30e-3,rangefinder_sampling_ratio = 1.,odometry_sampling_ratio = 1.,fixed_frame_pose_sampling_ratio = 1.,imu_sampling_ratio = 1.,landmarks_sampling_ratio = 1.,
}MAP_BUILDER.use_trajectory_builder_2d = trueTRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
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-1POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65return options

3、编译
catkin_make和catkin_make_isolated都可以，区别是后者使用一个隔离的构建过程，其中每个包都是独立配置、构建和加载到环境中的，解决了目标冲突、目标依赖项管理和项目之间其他不希望的交叉对话的问题，缺点是比较慢，具体参考这篇博客。
tips：编译后再修改launch或lua文件，只编译单独的包即可

 catkin_make_isolated --pkg cartographer_ros

4、跑一跑
先跑cartographer再跑rplidar_ros，如果反了，似乎不太行，原因还没有深究。

# 首先的首先，配置环境变量. /opt/ros/noetic/setup.bash. devel_isolated/setup.bash     # 因为只编译，没有安装，所以没有install文件夹# 首先运行 cartographerroslaunch cartographer_ros cartoForRplidar.launch# 然后运行 rplidar_rosroslaunch rplidar_ros test_rplidar.launch

出图：

3 ROS2下测试SLAM

3.1 cartographer 安装测试（失败）

ROS2_Foxy上使用cartographer有两种方式：命令行安装和源码编译安装。

# 命令行sudo apt install ros-foxy-cartographer
#源码https://github.com/cartographer-project/cartographer_ros/tree/ros2-dashing

cartographer似乎正在开发一个新版本（点击了解详情），直到去年11月还在开发中…

3.2 rplidar_ros 移植到 ROS2

参考这里：ros2激光雷达功能包移植（思岚rplidar A1）。
1、下载代码，然后找到rplidar_ros的包，并拷贝到自己的工作空间的src目录下

 git clone https://github.com/DylanLN/oryxbot_ws-ros2.git

2、编译、测试

# 编译. /opt/ros/foxy/setup.bashcolcon build# 跑一跑. install/setup.bashsudo chmod 777 /dev/ttyUSB0 ros2 launch rplidar_ros rplidar_A1.launch.py

另开一个终端，跑rviz2

 . /opt/ros/foxy/setup.bash. install/setup.bashrviz2

出图：
因为cartographer还没有成功在ROS2上跑，所以这里没有跑slam，等后面官方移植完善了，再看看。

4 ROS1 & ROS2下测试SLAM

好了，现在到了先导篇，最奇怪的一部分了：在ROS2上跑rplidar_ros，然后通过ros1_bridge把激光数据传到ROS1，然后在ROS1中运行cartographer进行建图。
按照前言中讲述的内容，激光雷达与cartographer都在ROS1环境下，车辆与电脑之间应该传递的是单车slam建图的结果，那么这里为什么把激光雷达放到ROS2下，然后在ROS1中接收ROS2的激光数据进行建图呢？因为不想在树莓派上，装完ROS2，再装ROS1，所以车子的作用变成了只采集激光数据，然后通过ROS2传到电脑，电脑端在ROS1中进行SLAM。
也许，后面会尝试在树莓派上再装个ROS1？

4.1 ros1_bridge

1、小前言
ros1_bridge是ROS2为了兼容ROS1而开发的包，这里介绍下基本的使用方法，然后将其应用到项目中。学习就是看看官网教程，这个包的安装过程可以命令行直接安装。

 sudo apt install ros-foxy-ros1-bridge

2、跑一跑（要开四个终端）

# Shell A (ROS 1 only):
. /opt/ros/noetic/setup.bash
roscore# Shell B (ROS 1 + ROS 2):
# Source ROS 1 first:
. /opt/ros/noetic/setup.bash
# Source ROS 2 next:
. /opt/ros/foxy/setup.bash
export ROS_MASTER_URI=http://localhost:11311   # 这里根据 Shell A 实际情况改
ros2 run ros1_bridge dynamic_bridge# Shell C:
. /opt/ros/noetic/setup.bash
rosrun rospy_tutorials talker# Shell D:
. /opt/ros/foxy/setup.bash
ros2 run demo_nodes_cpp listener

出图：

4.2 ROS1 & ROS2 & cartographer

1、ROS1下，跑cartographer，这会同时启动roscore

 . /opt/ros/noetic/setup.bash. devel_isolated/setup.bashroslaunch cartographer_ros cartoForRplidar.launch

2、ROS1和ROS2下，跑ros1_bridge

 . /opt/ros/noetic/setup.bash. /opt/ros/foxy/setup.bashexport ROS_MASTER_URI=http://localhost:11311 ros2 run ros1_bridge dynamic_bridge

3、ROS2下，跑rplidar_ros

 . /opt/ros/foxy/setup.bash. install/setup.bashros2 launch rplidar_ros2 rplidar_A1.launch.py# 这个包名叫rplidar_ros2而非rplidar_ros，是因为修改了launch文件，'frame_id': 'laser'# 以便于在下一部分与cartographer的frame对接正确

4、出图
可以看到，ROS1的cartographer拿到数据正在建图，第二张图右上角的终端显示两个ROS正在通过话题sensor_msgs/LaserScan通信。

参考

1、ROS(Kinetic和Melodic)下使用cartographer踩坑记录和部分谣言终结
2、catkin_make, cmake, catkin build区别
3、ros2激光雷达功能包移植（思岚rplidar A1）