cartgrapher这个框架是google在2016年开源出来的框架，该框架可以接入2D激光、3D激光、里程计、IMU传感器的数据，输出2D地图或者是3D地图。同时该框架还有一个比较有特色的功能就是他可以增量式更新地图，当cartgrapher 运行在定位模式时，可以在定位的同时增量式更新已有的地图。
1 cartography安装
网络上有很多安装cartography的教程[2]，基本上按照这些教程或者是官网提供的教程不存在其他问题，唯一的问题就是直接从github上拷贝的速度很慢，可以使用国内别克隆下来的代码。
根据官网的方式下载源码[1] 在下载的时候需要修改ceres-solver地址为：https://github.com/ceres-solver/ceres-solver.git （使用命令vim src/ .rosinstall）
或者去我的仓库进行下载，将src目录下的三个压缩包进行解压。
2 2D建图测试
这里我们首先测试官网上的demo,然后再我们自己的机器人上进行测试。这里我们运行的是官网上的 Pure localization 部分的数据集。
2.1 启动2D 建图demo
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=/media/crp/0E3C06880E3C0688/b2-2016-04-05-14-44-52.bag
bag_filename表示的是ROS bag的数据包。
注意：这里我们把官网上的 offline_backpack_2d.launch 文件替换为了 demo_backpack_2d.launch，这是由于我们发现 offline_backpack_2d.launch 这个文件没有启动地图保存服务。
cartograph_数据集2D建图(x8)
先将地图保存为 .pbstream 文件
rosservice call /write_state ~/cartograph_test.pbstream
用cartography自带的转换节点将.pbstream 文件转化为pgm和yaml文件
rosrun cartographer_ros cartographer_pbstream_to_ros_map -pbstream_filename /home/crp/cartograph_test.pbstream -map_filestem /home/crp/cartograph_test
启动节点以后可以看到文件夹下生成的pgm 和 yaml文件
但是如果你需要使用cartography进行定位的话，就没有必要去转换为pgm格式的。
2.2 启动2D 定位demo
接下来我们使用已有的地图进行定位
roslaunch cartographer_ros demo_backpack_2d_localization.launch load_state_filename:=/home/crp/ cartograph_test.pbstream bag_filename:=/media/crp/0E3C06880E3C0688/b2-2016-04-27-12-31-41.bag
其中cartograph_test.pbstream 是我们上一个步骤中生成的一个地图文件，bag_filename:表示的是当前输入的激光雷达的数据
cartograph_数据集2D定位增量式更新地图
其中定位数据是输出在TF坐标系中的。
2.3 在kobuki机器人上实现建图
这里我们是参考demo的历程来配置参数文件的，这里主要需要注意lua文件中的几个坐标系的配置。经过我自己的尝试
a) 在只使用激光雷达的时候（tracking_frame=”laser”, publish_frame=”laser”）
b) 使用里程计+激光雷达时（tracking_frame=”base_link”, publish_frame=”odom”）
c) 使用IMU+激光+里程计时（tracking_frame=”imu_link”, publish_frame=”odom”）
其余参数只要参考demo里面的进行配置就可以了，我所使用的launch文件（” kobuki_robot.launch”）和lua(“kobuki_robot.lua”)文件配置如下：
kobuki_robot.launch
<launch>
<arg name="urdf_file" default="$(find xacro)/xacro --inorder '$(find kobuki_description)/urdf/kobuki_standalone.urdf.xacro'"/>
<param name="robot_description" command="$(arg urdf_file)"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="5.0"/>
</node>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="use_gui" value="true"/>
</node>
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename kobuki_robot.lua"
output="screen">
<remap from="scan" to="/scan" />
<remap from="odom" to="/odom" />
</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>
kobuki_robot.lua
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_footprint",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false, --算法内部提供里程计
publish_frame_projected_to_2d = false,
use_odometry = true, --使用里程计
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 = true
TRAJECTORY_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.imu_gravity_time_constant = 9.8
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-1
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65
return options
下面是一个在实验室里面录制的视频
kobuki cartograph 建图
2.4 在kobuki上基于已有地图定位
接下来我们使用以及建立好的地图进行定位，同时进行增量式更新地图（注意地图右上角区域）
kobuki_localization.launch
<launch>
<arg name="urdf_file" default="$(find xacro)/xacro --inorder '$(find kobuki_description)/urdf/kobuki_standalone.urdf.xacro'"/>
<param name="robot_description" command="$(arg urdf_file)"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
<param name="publish_frequency" type="double" value="5.0"/>
</node>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="use_gui" value="true"/>
</node>
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename kobuki_localization.lua
-load_state_filename $(arg load_state_filename)"
output="screen">
<remap from="echoes" 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>
kobuki_localization.lua 定位模式下的lua配置文件，只是在建图的基础上增加了两个配置参数
include "kobuki_robot.lua"
TRAJECTORY_BUILDER.pure_localization = true
POSE_GRAPH.optimize_every_n_nodes = 20
return options
定位&增量更新地图视频：
kobuki_Cartograph_定位&增量式建图
3、3D建图测试
3.1 3D数据集建图
使用3D激光雷达建图的时候我们必须要结合IMU，使用IMU提供的重力方向向量。这里我们直接根据官网[1] 的步骤进行运行，首先你需要去下载这个3D数据包[5]
其次我们需要将官网上的 “offline_backpack_3d.launch”替换为 “demo_backpack_3d.launch”,否则在保存地图的时候会出现无法调用
启动3D激光雷达建图
roslaunch cartographer_ros demo_backpack_3d.launch bag_filename:=/media/crp/0E3C06880E3C0688/b3-2016-04-05-13-54-42.bag
Cartography-3D建图
等到数据运行完毕以后调用 write_state 服务来保存地图
rosservice call /write_state ~/3d_local.pbstream
将这个pbstream文件进一步转化成3D的ply点云文件
roslaunch cartographer_ros assets_writer_my_rslidar_3d.launch bag_filenames:=b3-2016-04-05-13-54-42.bag pose_graph_filename:=~/3d_local.pbstream
等待一段时间，处理完成后命令会自动退出，此时在bag文件旁边会生成一个.bag_points.ply后缀文件，这个就是点云文件[6].最后利用PCL自带的工具将ply文件转换成pcd文件
pcl_ply2pcd b3-2016-04-05-13-54-42.bag_points.ply test_3d.pcd
在运行的时候，机器人的位姿是发布在TF中的，如下图所示。因此我们可以通过读取odom->map之间的坐标变换来知道机器人的位置
3.2 3D定位
3D定位我们是利用在3.1部分生成的 “***~/3d_local.pbstream***”作为已有地图，将当前激光数据输入进行匹配，估计位置
cartographer_ros demo_backpack_3d_localization.launch load_state_filename:=/home/crp/3d_local.pbstream bag_filename:=/media/crp/0E3C06880E3C0688/b3-2016-04-05-15-52-20.bag
Cartography-3D定位
同样在运行定位的时候，机器人的位姿也是发布在TF中的，如下图所示。因此我们可以通过读取odom->map之间的坐标变换来知道机器人的位置。（可以明显看出，定位时候的位姿输出频率要远远低于建图时候的频率）
参考资料：
[1]https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html#building-installation
[2] https://www.cnblogs.com/hitcm/p/5939507.html
[3] https://zhuanlan.zhihu.com/p/64747755
[4] https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-13-54-42.bag
[5] https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-15-52-20.bag
[6]http://community.bwbot.org/topic/523/%E8%B0%B7%E6%AD%8Ccartographer%E4%BD%BF%E7%94%A8%E9%80%9F%E8%85%BE%E8%81%9A%E5%88%9B3d%E6%BF%80%E5%85%89%E9%9B%B7%E8%BE%BE%E6%95%B0%E6%8D%AE%E8%BF%9B%E8%A1%8C%E4%B8%89%E7%BB%B4%E5%BB%BA%E5%9B%BE/2
欢迎做自主导航小车的小伙伴一起讨论或者留言，[email protected]