cartography框架安装与建图测试

cartgrapher这个框架是google在2016年开源出来的框架，该框架可以接入2D激光、3D激光、里程计、IMU传感器的数据，输出2D地图或者是3D地图。同时该框架还有一个比较有特色的功能就是他可以增量式更新地图，当cartgrapher 运行在定位模式时，可以在定位的同时增量式更新已有的地图。

cartographer安装

网络上有很多安装cartographer的教程：

https://www.cnblogs.com/hitcm/p/5939507.html

基本上按照这些教程或者是官网提供的教程不存在其他问题，唯一的问题就是直接从github上拷贝的速度很慢，可以使用国内别克隆下来的代码。

根据官网的方式下载源码：

https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html#building-installation

在下载的时候需要修改ceres-solver地址为：

https://github.com/ceres-solver/ceres-solver.git （使用命令vim src/ .rosinstall）

或者去我的仓库：（https://gitee.com/cenruping/google_cartgrapher）进行下载，将src目录下的三个压缩包进行解压。

2D建图测试

这里我们首先测试官网上的demo,然后再我们自己的机器人上进行测试。这里我们运行的是官网上的 Pure localization 部分的数据集。

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 这个文件没有启动地图保存服务。

先将地图保存为 .pbstream 文件

rosservice call /write_state ~/cartograph_test.pbstream

用cartographer自带的转换节点将.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文件

但是如果你需要使用cartographer进行定位的话，就没有必要去转换为pgm格式的。

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:表示的是当前输入的激光雷达的数据

其中定位数据是输出在TF坐标系中的。

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

  "urdf_file" default="$(find xacro)/xacro --inorder '$(find kobuki_description)/urdf/kobuki_standalone.urdf.xacro'"/> "robot_description" command="$(arg urdf_file)"/>
 "robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">  "publish_frequency" type="double" value="5.0"/> 
 "joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">  "use_gui" value="true"/> 

 "cartographer_node" pkg="cartographer_ros"   type="cartographer_node" args="     -configuration_directory $(find cartographer_ros)/configuration_files     -configuration_basename kobuki_robot.lua"   output="screen">  "scan" to="/scan" />  "odom" to="/odom" /> 

 "cartographer_occupancy_grid_node" pkg="cartographer_ros"   type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
 "rviz" pkg="rviz" type="rviz" required="true"   args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />

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 = 35TRAJECTORY_BUILDER_2D.min_range = 0.3TRAJECTORY_BUILDER_2D.max_range = 8.TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.TRAJECTORY_BUILDER_2D.use_imu_data = falseTRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 9.8 TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = trueTRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1TRAJECTORY_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 = 1e2POSE_GRAPH.optimize_every_n_nodes = 35POSE_GRAPH.constraint_builder.min_score = 0.65
return options

下面是一个在实验室里面录制的视频：

4.在kobuki上基于已有地图定位

接下来我们使用以及建立好的地图进行定位，同时进行增量式更新地图（注意地图右上角区域）kobuki_localization.launch

  "urdf_file" default="$(find xacro)/xacro --inorder '$(find kobuki_description)/urdf/kobuki_standalone.urdf.xacro'"/> "robot_description" command="$(arg urdf_file)"/>
 "robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">  "publish_frequency" type="double" value="5.0"/> 
 "joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">  "use_gui" value="true"/> 

 "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">  "echoes" to="/scan" /> 

 "cartographer_occupancy_grid_node" pkg="cartographer_ros"   type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
 "rviz" pkg="rviz" type="rviz" required="true"   args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />

kobuki_localization.lua 定位模式下的lua配置文件，只是在建图的基础上增加了两个配置参数

include "kobuki_robot.lua"
TRAJECTORY_BUILDER.pure_localization = truePOSE_GRAPH.optimize_every_n_nodes = 20
return options

定位&增量更新地图视频：

3D建图测试

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

等到数据运行完毕以后调用 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之间的坐标变换来知道机器人的位置

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

同样在运行定位的时候，机器人的位姿也是发布在TF中的，如下图所示。因此我们可以通过读取odom->map之间的坐标变换来知道机器人的位置。（可以明显看出，定位时候的位姿输出频率要远远低于建图时候的频率）

审核编辑：汤梓红