如何使用 Google Cartographer SLAM 演算法來建地圖

Posted 2023-04-03

参考技术A 不用生成直接有现成的Google Earth，谷歌投资了几十个天上飞的玩意弄出来的，比某度神马的强 参考技术B 建立模拟环境
我的测试环境是 Ubuntu 14.04 + ROS Indigo，不过使用的指令都很 general，即便你使用其他版本，应该也不会有什麼问题。
首先我们确定已经安装所需要的套件：
sudo apt-get install ros-indigo-gazebo-*

接下来在终端机输入 gazebo 应该就可以成功启动 Gazebo 模拟器。然後我们可以用现成的 package 启动一个有 PR2 的复杂环境。
roslaunch pr2_gazebo pr2_wg_world.launch

理论上，你会碰到一个 error 说：
unused args [paused, gui] for include of [/opt/ros/jade/share/gazebo_ros/launch/willowgarage_world.launch] The traceback for the exception was written to the log file

这时候只要去编辑你的 pr2_wg_world.launch，把有 arg 的部分注解掉就好，注解完的档案看起来会像下面这样：
<launch>

<!-- start up empty world -->
<!-- <arg name="gui" default="true"/>
<arg name="paused" default="true"/> -->
<!-- TODO:throttled not implemented in gazebo_ros/empty_world.launch
<arg name="throttled" default="false"/>
-->

<includefile="$(find gazebo_ros)/launch/willowgarage_world.launch">
<!-- <arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)" /> -->
<!-- TODO:throttled not implemented in gazebo_ros/empty_world.launch
<arg name="throttled" value="$(arg throttled)" />
-->
</include>

<!-- start pr2 robot -->
<includefile="$(find pr2_gazebo)/launch/pr2.launch"/>

</launch>

注解完毕就可以启动啦。启动之後的环境会长得像下面这样：

安装 & 启动 Cartographer
安装的方法满简单的，只要照著官方文件上的步骤做就可以了：
# Install wstool and rosdep.
sudo apt-get update
sudo apt-get install -y python-wstool python-rosdep ninja-build

# Create a new workspace in 'catkin_ws'.
mkdir catkin_ws
cd catkin_ws
wstool init src

# Merge the cartographer_ros.rosinstall file and fetch code for dependencies.
wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall
wstool update -t src

# Install deb dependencies.
rosdep init
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro=$ROS_DISTRO -y

# Build and install.
catkin_make_isolated --install --use-ninja
source install_isolated/setup.bash

重点是装完之後，我们想要跑 cartographer_ros package 里面的 demo_pr2.launch，但里面原本预设是跑 bag file 的 demo 版本，所以我们要稍微修改一下 demo_pr2.launch ：
rosed cartographer_ros demo_pr2.launch

<!--
Copyright 2016 The Cartographer Authors

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->

<launch>
<paramname="/use_sim_time"value="true"/>

<nodename="cartographer_node"pkg="cartographer_ros"
type="cartographer_node"args="
-configuration_directory
$(find cartographer_ros)/configuration_files
-configuration_basename pr2.lua"
output="screen">
<remapfrom="scan"to="/base_scan"/>
</node>

<nodename="tf_remove_frames"pkg="cartographer_ros"
type="tf_remove_frames.py">
<remapfrom="tf_out"to="/tf"/>
<rosparamparam="remove_frames">
- map
- odom_combined
</rosparam>
</node>

<nodename="rviz"pkg="rviz"type="rviz"required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz"/>
</launch>

修改完之後就可以执行了，这时应该可以看到 Rviz 的视窗跳出来，也可以看到目前的地图。
roslaunch cartographer_ros demo_pr2.launch

Teleop PR2 来建地图罗
只要启动 teleop_keyborad 就可以用键盤控制机器人移动：
roslaunch pr2_teleop teleop_keyboard.launch

WASD 四个按键分别代表前左後右四个方向的平移、QE两个按键是原地旋转，这边要注意必须选到启动 teleop_keyboard的视窗，按按键才有用。使用 teleop 来控制 PR2 走一走之後，就可以看得出我们已经建立了一个有模有样的地图。不过因为这个地图太大，我就不走完了。本回答被提问者采纳

cartographer源码解析

cartographer为Google提供的激光SLAM开源库，通常通过其提供的ROS平台封装进行使用，该库结构清晰，模块完整，值得深入研究。

项目官网：https://google-cartographer.readthedocs.io/en/latest/

项目Github：https://github.com/cartographer-project/cartographer

环境安装与配置具体参考：https://www.cnblogs.com/lvchaoshun/p/9824528.html

安装过程中可能会碰到一些版本冲突之类的问题，耐心上网寻求解决方法。

温馨提示：本系列主要介绍源码内容，不介绍环境搭建，仅作为自己学习过程记录，可能会比较乱，有疑问欢迎留言讨论。

下面进入正题，下图是cartographer官网提供的软件架构图，本系列将通过cartographer_ros封装库中调用顺序，对程序主线进行介绍，暂未使用到的或次要的信息以后有空再分析。

 

---

 首先我们找到cartographer_ros中程序入口，即cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc，其中的main函数为cartographer_ros节点启动的程序入口。

 其他都是ros平台初始化相关的函数，只用看98行Run方法。

可以看到该函数内主要是些初始化工作，创建map_builder传给node进行初始化、读取pbstream地图文件，并调用StartTrajectoryWithDefaultTopics函数，该函数定义在node.cc中。

 我们会发现AddTrajectory是核心功能入口。××该函数需重点关注，之后我们还会回来一个个分析其中调用的关键函数。

 

 该函数功能比较多，我们一步步看。首先363行创建一个set保存传感器话题的编号，用于之后订阅器订阅。366行调用map_builder_bridge_实例的AddTrajectory函数，在cartographer_ros中类名包含“bridge”的类基本上都是作为“桥梁”调用cartographer源码的类。我们可以看到map_builder_bridge_为map_builder_bridge.cc中定义的MapBuilderBridge的实例，此时关注map_builder_bridge.cc中定义的AddTrajectory函数内容。

 根据cartographer全局配置参数和最初Run方法中定义的map_builder实例类型，我们了解到map_builder_为cartographer/mapping/map_builder.cc中定义的MapBuilder类的实例。上图函数内容分为两块，一块调用map_builder_的AddTrajectoryBuilder函数，创建trajectory_builder并设置结果回调，返回一个路径id作为当前轨迹的编号，详情下篇再介绍；另一块主要初始化一个SensorBridge实例，并放入sensor_birdges_这个map中，以及保存该路径id对应的配置参数。

接下来我们回到node.cc中核心功能入口函数。368与369行分别创建与当前路径编号对应的位姿预测器(PoseExtrapolator)与传感器采样器(TrajectorySensorSamplers)，根据配置参数创建实例没啥好说的。371行启动ros话题订阅器，并设置回调函数。这之后的创建定时器和记录话题编号也很简单。我们主要关注话题订阅的回调函数。

上图截取了LaunchSubsribers函数的部分代码，通过其回调函数，我们发现最终会调用sensor_bridge.cc中定义的HandleLaserScan函数。此处仅仅举个例子，该函数仅处理激光雷达数据(普通LaserScan类型消息、多回波、点云等)，其他传感器数据(如IMU、里程计等)会调用不同的回调函数。

我们观察HandleLaserScan函数实现。

 首先对点云细分，默认num_subdivisions_per_laser_scan_=10。points对象的类型为cartographer_ros自定义类型，其中保存了每个点的光强、位置与时间增量。time_to_subdivision_end保存了最后一个点的时间增量，即一块细分点云间时间间隔(第一个点之前的时刻到最后一个点之后的时刻之间的时间间隔)。之后做一些时间戳的运算，或许是诸如(..., -0.06, -0.04, -0.02, 0)的一组时间戳。最后调用HandleRangefinder将数据添加到全局路径构造器，如下图所示。

此处trajectory_builder_会是GlobalTrajectoryBuilder类的实例，是由于map_builder_的AddTrajectoryBuilder函数中调用了对应的构造函数，此处已涉及cartographer源码。

 本节主要介绍了cartographer_ros封装是如何跳转到cartographer源码中的，主要包括ros节点、map_builder与trajectory_builder的创建，pose_extrapolator与trajectory_sensor_samplers的初始化以及传感器话题订阅与处理。从下一节开始，将根据调用顺序介绍cartographer源码。