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]

cartography在機器人上運行建圖與定位

1 cartography安裝

2 2D建圖測試

2.1 啓動2D 建圖demo

2.2 啓動2D 定位demo

2.3 在kobuki機器人上實現建圖

2.4 在kobuki上基於已有地圖定位

3、3D建圖測試

3.1 3D數據集建圖

3.2 3D定位

參考資料：

PDManer [元數建模]-v4.9.0 發佈：一款簡單好用的數據庫建模平臺

使用neovim打造go ide(支持代碼跳轉, 代碼補全, 實時語法檢查)

cs01 CSS Syntax

挑戰程序設計競賽 2.3章習題 poj 3046 Ant Counting

[MASM拾遺]Offset僞指令

h30 HTML Layout Elements

瞭解顯卡

一款基於C#開發的通訊調試工具（支持Modbus RTU、MQTT調試）

Linux/Golang/glibC系統調用

cs04 CSS Measurement Units

ROS中使用羅技G29遙控器

SLAM 軌跡評估工具EVO

INDEMIND 雙目相機使用教程

ROS麥克納姆輪底盤製作（上）

Webots 機器人仿真平臺(一) 系統安裝

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結