cartgrapher這個框架是google在2016年開源出來的框架，該框架可以接入2D激光、3D激光、里程計、IMU傳感器的數(shù)據(jù)，輸出2D地圖或者是3D地圖。同時該框架還有一個比較有特色的功能就是他可以增量式更新地圖，當(dāng)cartgrapher 運行在定位模式時，可以在定位的同時增量式更新已有的地圖。

cartographer安裝

網(wǎng)絡(luò)上有很多安裝cartographer的教程：

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

基本上按照這些教程或者是官網(wǎng)提供的教程不存在其他問題，唯一的問題就是直接從github上拷貝的速度很慢，可以使用國內(nèi)別克隆下來的代碼。

根據(jù)官網(wǎng)的方式下載源碼：

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）進(jìn)行下載，將src目錄下的三個壓縮包進(jìn)行解壓。

2D建圖測試

這里我們首先測試官網(wǎng)上的demo,然后再我們自己的機器人上進(jìn)行測試。這里我們運行的是官網(wǎng)上的 Pure localization 部分的數(shù)據(jù)集。

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的數(shù)據(jù)包。

注意：

這里我們把官網(wǎng)上的offline_backpack_2d.launch 文件替換為了 demo_backpack_2d.launch，這是由于我們發(fā)現(xiàn) offline_backpack_2d.launch 這個文件沒有啟動地圖保存服務(wù)。

先將地圖保存為 .pbstream 文件

rosservice call /write_state ~/cartograph_test.pbstream

用cartographer自帶的轉(zhuǎn)換節(jié)點將.pbstream 文件轉(zhuǎn)化為pgm和yaml文件

rosrun cartographer_ros cartographer_pbstream_to_ros_map -pbstream_filename /home/crp/cartograph_test.pbstream -map_filestem /home/crp/cartograph_test

動節(jié)點以后可以看到文件夾下生成的pgm 和 yaml文件

但是如果你需要使用cartographer進(jìn)行定位的話，就沒有必要去轉(zhuǎn)換為pgm格式的。

2.啟動2D 定位demo

接下來我們使用已有的地圖進(jìn)行定位

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:表示的是當(dāng)前輸入的激光雷達(dá)的數(shù)據(jù)

其中定位數(shù)據(jù)是輸出在TF坐標(biāo)系中的。

3.在kobuki機器人上實現(xiàn)建圖

這里我們是參考demo的歷程來配置參數(shù)文件的，這里主要需要注意lua文件中的幾個坐標(biāo)系的配置。經(jīng)過我自己的嘗試

a) 在只使用激光雷達(dá)的時候（tracking_frame=”laser”, publish_frame=”laser”）

b) 使用里程計+激光雷達(dá)時（tracking_frame=”base_link”, publish_frame=”odom”）

c) 使用IMU+激光+里程計時（tracking_frame=”imu_link”, publish_frame=”odom”）

其余參數(shù)只要參考demo里面的進(jìn)行配置就可以了，我所使用的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, --算法內(nèi)部提供里程計 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上基于已有地圖定位

接下來我們使用以及建立好的地圖進(jìn)行定位，同時進(jìn)行增量式更新地圖（注意地圖右上角區(qū)域）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配置文件，只是在建圖的基礎(chǔ)上增加了兩個配置參數(shù)

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

定位&增量更新地圖視頻：

3D建圖測試

1.3D數(shù)據(jù)集建圖

使用3D激光雷達(dá)建圖的時候我們必須要結(jié)合IMU，使用IMU提供的重力方向向量。這里我們直接根據(jù)官網(wǎng)[1] 的步驟進(jìn)行運行，首先你需要去下載這個3D數(shù)據(jù)包[5]

其次我們需要將官網(wǎng)上的 “offline_backpack_3d.launch”替換為 “demo_backpack_3d.launch”,否則在保存地圖的時候會出現(xiàn)無法調(diào)用

啟動3D激光雷達(dá)建圖

roslaunch cartographer_ros demo_backpack_3d.launch bag_filename:=/media/crp/0E3C06880E3C0688/b3-2016-04-05-13-54-42.bag

等到數(shù)據(jù)運行完畢以后調(diào)用 write_state 服務(wù)來保存地圖

rosservice call /write_state ~/3d_local.pbstream

將這個pbstream文件進(jìn)一步轉(zhuǎn)化成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文件轉(zhuǎn)換成pcd文件

pcl_ply2pcd b3-2016-04-05-13-54-42.bag_points.ply test_3d.pcd

在運行的時候，機器人的位姿是發(fā)布在TF中的，如下圖所示。因此我們可以通過讀取odom->map之間的坐標(biāo)變換來知道機器人的位置

2.3D定位

3D定位我們是利用在3.1部分生成的 “***~/3d_local.pbstream***”作為已有地圖，將當(dāng)前激光數(shù)據(jù)輸入進(jìn)行匹配，估計位置

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

同樣在運行定位的時候，機器人的位姿也是發(fā)布在TF中的，如下圖所示。因此我們可以通過讀取odom->map之間的坐標(biāo)變換來知道機器人的位置。（可以明顯看出，定位時候的位姿輸出頻率要遠(yuǎn)遠(yuǎn)低于建圖時候的頻率）