*转载请注明出处 @梦凝小筑
前言
本人研一主要研究室内2D激光SLAM，对应的项目是激光AGV。现在项目也已经完结，这里分享下Cartographer用于机器人定位的经验。
我看到大部分使用Cartographer的人只用Cartographer进行建图，并没有用其进行定位。大多还是用的Cartographer + AMCL + MoveBase这一套。本人一开始也是这样做的，但后来发现AMCL定位精度比较差，而且已经废了很大劲学习Cartographer，为什么不用他来进行定位呢？于是现在使用的是Cartographer + MoveBase，把 AMCL 去掉了。
后面才了解到SLAM（同步定位与建图），建图的过程中便是 定位 + 地图叠加，因此Cartographer可用于建图，必然可用于定位，事实的确如此。Cartographer源码里其实也开放了纯定位的接口，只是初学的话一般很难运用。
Cartographer纯定位接口：
文件：backpack_2d_localization.lua:
TRAJECTORY_BUILDER.pure_localization_trimmer = {
max_submaps_to_keep = 3, //最大保存子图数，存定位模式通过子图进行定位，但只需要当前和上一个子图即可，我这里设置的是2
}
POSE_GRAPH.optimize_every_n_nodes = 20 //每20个有效帧一个子图，子图构建完成要闭环检测一次，这个数越小，闭环检测越频繁，当然CPU爆炸
一、纯定位仿真测试
官方文档提供了纯定位的仿真教程，步骤如下：
下载数据集
wget -P ~/Downloads https://storage.useso.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-05-14-44-52.bag
wget -P ~/Downloads https://storage.useso.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-27-12-31-41.bag
建图
roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag
纯定位测试
这里后面两个参数一个是上一步建好的地图，pbstream格式，另一个是数据集
roslaunch cartographer_ros demo_backpack_2d_localization.launch \
load_state_filename:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream \
bag_filename:=${HOME}/Downloads/b2-2016-04-27-12-31-41.bag
也就是说，先用第一个数据集建图，建完图后用第二个数据集进行定位测试。
二、纯定位用在自己的机器人上
能看这篇文章相信已经掌握了Cartographer建图的方法。其实想让Cartographer用于定位也很简单。步骤如下：
修改文件 demo_backpack_2d_localization.launch
按照下面的文档中序号修改即可，不要直接粘贴。这个lannch文件建议大家也备份一个
<launch>
<!-- <param name="/use_sim_time" value="true" /> -->
<param name="/use_sim_time" value="false" /> <!--//1.仿真为true-->
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/my_backpack_2d.urdf" /> <!--//2.修改机器人模型-->
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename my_backpack_2d_localization.lua //3.纯定位接口参数设置
<!-- -load_state_filename $(arg load_state_filename)" -->
-load_state_filename /home/yourname/Map/map.pbstream" <!--//4.纯定位使用的地图，我是直接用固定路径,这里按照自己的路径修改即可-->
output="screen">
<!-- <remap from="echoes" to="horizontal_laser_2d" /> --> <!--//这个好像没啥用，我们一般用的都是 scan-->
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05
-pure_localization 1" /> <!--//5.修改文件occupancy_node_main.cc 增加是否纯定位模式参数pure_localization（如果选择直接注释就不需要增加这个参数了）-->
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
<!--<node name="playbag" pkg="rosbag" type="play"
args="--clock $(arg bag_filename)" /> --> <!--//6.不需要跑数据集，直接打开自己的雷达-->
</launch>
修改说明：
1.仿真为true，实际测试false
2.修改机器人模型 这里建议新建一个 my_backpack_2d.urdf
里面设置自己的机器人模型，下面是我的
<robot name="cartographer_backpack_2d">
<material name="orange">
<color rgba="1.0 0.5 0.2 1" />
</material>
<material name="gray">
<color rgba="0.2 0.2 0.2 1" />
</material>
<link name="imu_link">
<visual>
<origin xyz="0 0 0" />
<geometry>
<box size="0.06 0.04 0.02" />
</geometry>
<material name="orange" />
</visual>
</link>
<link name="base_scan">
<visual>
<origin xyz="0 0 0" />
<geometry>
<cylinder length="0.05" radius="0.03" />
</geometry>
<material name="gray" />
</visual>
</link>
<link name="base_footprint" />
<joint name="imu_link_joint" type="fixed">
<parent link="base_footprint" />
<child link="imu_link" />
<origin xyz="0 0 0" />
</joint>
<joint name="base_scan_joint" type="fixed">
<parent link="base_footprint" />
<child link="base_scan" />
<origin xyz="0.160 0 0.0558" />
</joint>
</robot>
上面第一行引用的文件是 my_backpack_2d.lua，这个文件是backpack_2d.lua修改过来的，配置Cartographer在定位过程中的其它参数的，这里也建议新建一个my_backpack_2d.lua，用于区分建图时用的参数。
4.纯定位使用的地图，我是直接用固定路径,这里按照自己的路径修改即可
5.修改文件occupancy_node_main.cc
这个文件在路径cartographer_ros-master\cartographer_ros\cartographer_ros\occupancy_grid_node_main.cc
第171行或者直接搜索下面这行代码：
//occupancy_grid_publisher_.publish(*msg_ptr);
注释掉这行代码，不然你纯定位过程建立的子图会覆盖在之前建好的地图上。
但是这行代码在建图的时候又肯定的是需要的，我的解决方法是引入外部参数进行切换。
修改文件在https://github.com/lllray/file/blob/master/occupancy_grid_node_main.cc
6.不需要跑数据集，直接打开自己的雷达
因为是用在自己的机器人上，所以不需要数据集
一般的步骤是：
先开雷达驱动
再开机器人驱动
最后打开我们修改的demo_backpack_2d_localization.launch文件
补充说明：
如果上述步骤失败的，检查下自己的TF变换是否正确，TOPIC是否对上了。
ROS用在机器人导航上就这些问题，只要TOPIC对上了，TF变换正确，基本都没啥问题，这也正是ROS的强大之处。
三、定位结果
我觉的很好，比AMCL好多了，哈哈哈，你们可以试试。
有一个很强大之处，由于Cartographer定位是是当前帧和子图进行匹配，所以对于机器人周围被新物体遮挡的情况完全没有影响。
唯一影响就是没法重定位了，因为地图上没有这些新物体，如下面那一圈障碍。
但是就算长时间被遮挡，存在累积定位误差，只要机器人离开了遮挡区域，回到了之前的环境，就可以重定位回来。
因此Cartographer定位十分稳定，特别是一大堆领导来检查的时候（一般都会把可怜的机器人团团围住），完全没怕的，哈哈哈。
