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

cartographer安裝

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

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

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

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

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

在下載的時(shí)候需要修改ceres-solver地址為：

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

或者去我的倉(cāng)庫(kù)：（https://gitee.com/cenruping/google_cartgrapher）進(jìn)行下載，將src目錄下的三個(gè)壓縮包進(jìn)行解壓。

2D建圖測(cè)試

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

1.啟動(dòng)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 這個(gè)文件沒(méi)有啟動(dòng)地圖保存服務(wù)。

先將地圖保存為 .pbstream 文件

rosservice call /write_state ~/cartograph_test.pbstream

用cartographer自帶的轉(zhuǎn)換節(jié)點(diǎn)將.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

動(dòng)節(jié)點(diǎn)以后可以看到文件夾下生成的pgm 和 yaml文件

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

2.啟動(dòng)2D 定位demo

接下來(lái)我們使用已有的地圖進(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 是我們上一個(gè)步驟中生成的一個(gè)地圖文件，bag_filename:表示的是當(dāng)前輸入的激光雷達(dá)的數(shù)據(jù)

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

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

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

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

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

c) 使用IMU+激光+里程計(jì)時(shí)（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)部提供里程計(jì) publish_frame_projected_to_2d = false, use_odometry = true, --使用里程計(jì) 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

下面是一個(gè)在實(shí)驗(yàn)室里面錄制的視頻：

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

接下來(lái)我們使用以及建立好的地圖進(jìn)行定位，同時(shí)進(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ǔ)上增加了兩個(gè)配置參數(shù)

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

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

3D建圖測(cè)試

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

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

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

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

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

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

rosservice call /write_state ~/3d_local.pbstream

將這個(gè)pbstream文件進(jìn)一步轉(zhuǎn)化成3D的ply點(diǎn)云文件

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

等待一段時(shí)間，處理完成后命令會(huì)自動(dòng)退出，此時(shí)在bag文件旁邊會(huì)生成一個(gè).bag_points.ply后綴文件，這個(gè)就是點(diǎn)云文件[6].最后利用PCL自帶的工具將ply文件轉(zhuǎn)換成pcd文件

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

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

2.3D定位

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

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

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