Cartographer使用流程-建图-纯定位-导航

问题：
1、官方下载地址；
2、官方文档；

安装

参考博文https://blog.csdn.net/weixin_39784658/article/details/99452960
官方安装指导https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html#building-installation

跑数据集

此部分参考博文 http://www.freesion.com/article/9794248452/

下载数据集

wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-05-14-44-52.bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-27-12-31-41.bag

跑数据集建图

roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag

跑数据集纯定位测试

roslaunch cartographer_ros demo_backpack_2d_localization.launch \
   load_state_filename:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream \
   bag_filename:=${HOME}/Downloads/b2-2016-04-27-12-31-41.bag

这里后面两个参数一个是上一步建好的地图，pbstream格式，另一个是数据集。也就是说，先用第一个数据集建图，建完图后用第二个数据集进行定位测试。

主要修改文件及参数

依据原有参考文件新建文件

文件对应关系

backpack_2d.launch -> my_robot_2d.launch
demo_backpack_2d.launch -> demo_my_robot_2d.launch
offline_backpack_2d.launch -> offline_my_robot_2d.launch
demo_backpack_2d_localization.launch -> demo_my_robot_localization_2d.launch
assets_writer_backpack_2d.launch -> assets_writer_my_robot_2d.launch

backpack_2d.lua -> my_robot_2d.lua
backpack_2d_localization.lua -> my_robot_2d_localization.lua
aasets_writer_backpack_2d.lua -> aasets_writer_my_robot_2d.lua

建图

参考博文https://blog.csdn.net/m0_37672916/article/details/77198261

建图操作流程

主要包含：激光雷达、移动机器人（轮椅）、cartographer。

1、启动激光雷达

roslaunch urg_node urg_lidar.launch

2、启动轮椅

rosrun wheel_chair wheel_driver 

3、启动建图

 roslaunch cartographer_ros demo_revo_lds.launch 

备注：不需要里程信息，也可以建图；在建图过程中，轮椅编码器未转动。

可将如上流程写成launch文件：

主要注意：TF、Topic

TF-tree关系

map
 |----odom
        |----base_footprint(差动轮中心)
                 |---- base_link(轮椅)
                            |----base_scan(激光雷达)

坐标间关系建立

1、轮椅建立odom、base_footprint、base_link、base_scan三者之间的关系。
其中，
base_footprint、base_link、base_scan，三者之间为固定关系，可在launch文件中建立为静态坐标系，固定周期发布；
base_footprint相对于odom为里程关系，根据运动实时计算里程计推算的位姿，需要实时发布；
2、Cartographer查阅TF-tree上的base_scan相对于map的关系，根据激光点云定位base_scan，并根据base_link与base_scan的关系追踪base_link的位姿，最终推算odom相对于map的位姿关系并发布。简而言之，cartographer通过修整odom相对于map的关系，从而定位机器人base_link相对于map的关系。
在.lua配置文件中设定如下值：

map_frame = "map",
tracking_frame = "base_link",
published_frame = "odom",
odom_frame = "odom",

provide_odom_frame = false,
use_odometry = true,

保存地图

不再接受进一步数据

rosservice call /finish_trajectory 0

序列化保存其当前状态

rosservice call /write_state "{filename: '${HOME}/Downloads/mymap_name.pbstream'}"

将pbstream转换为pgm和yaml

rosrun cartographer_ros cartographer_pbstream_to_ros_map  -map_filestem=${HOME}/Downloads/mymap_name  -pbstream_filename=${HOME}/Downloads/mymap_name.pbstream -resolution=0.05

纯定位

启动文件：

导航

启动文件：

附件

主要更改的文件

• my_robot_2d_localization.lua
  文件功能：配置机器人纯定位的相关参数。保证定位过程中的低延时特性。

include "my_robot_2d.lua"

-- TRAJECTORY_BUILDER.pure_localization_trimmer = {
--     max_submaps_to_keep = 3, -- 最大保存子图数，存定位模式通过子图进行定位，但只需要当前和上一个子图即可，我这里设置的是2
--   }
TRAJECTORY_BUILDER.pure_localization = true
POSE_GRAPH.optimize_every_n_nodes = 20


-- output map to base_link for evaluation
-- options.provide_odom_frame = false
-- POSE_GRAPH.optimization_problem.log_solver_summary = true

-- fast localization
MAP_BUILDER.num_background_threads = 8
POSE_GRAPH.constraint_builder.sampling_ratio = 0.5 * POSE_GRAPH.constraint_builder.sampling_ratio
POSE_GRAPH.global_sampling_ratio = 0.1 * POSE_GRAPH.global_sampling_ratio
POSE_GRAPH.max_num_final_iterations = 1

return options

• my_robot_2d.lua
  文件功能：配置机器人相关参数，主要坐标系名称、是否有里程计/卫星/地标，传感器数量，传感器测量范围等。

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,

  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "base_link",
  odom_frame = "odom",

  provide_odom_frame = true,
  publish_frame_projected_to_2d = true,

  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 = 10,
  num_point_clouds = 0,

  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 10e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1e-3,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.min_range = 0.1,
TRAJECTORY_BUILDER_2D.max_range = 8.0,
TRAJECTORY_BUILDER_2D.min_z = -0.05,
TRAJECTORY_BUILDER_2D.max_z = 0.5,
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 8.0

POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight = 1e5
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight = 1e5
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e4
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e4

return options

• offline_my_robot_2d_localization.lua
  主要功能：离线定位测试，仅用单激光雷达，无里程计。

include "offline_my_robot_2d.lua"

-- TRAJECTORY_BUILDER.pure_localization_trimmer = {
--     max_submaps_to_keep = 3, -- 最大保存子图数，存定位模式通过子图进行定位，但只需要当前和上一个子图即可，我这里设置的是2
--   }
TRAJECTORY_BUILDER.pure_localization = true
POSE_GRAPH.optimize_every_n_nodes = 20


-- output map to base_link for evaluation
-- options.provide_odom_frame = false
-- POSE_GRAPH.optimization_problem.log_solver_summary = true

-- fast localization
MAP_BUILDER.num_background_threads = 8
POSE_GRAPH.constraint_builder.sampling_ratio = 0.5 * POSE_GRAPH.constraint_builder.sampling_ratio
POSE_GRAPH.global_sampling_ratio = 0.1 * POSE_GRAPH.global_sampling_ratio
POSE_GRAPH.max_num_final_iterations = 1

return options

• offline_my_robot_2d.lua
  文件功能：离线机器人参数，仅有单线激光雷达。

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,

  map_frame = "map",
  tracking_frame = "laser_frame",
  published_frame = "laser_frame",
  odom_frame = "odom",

  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,

  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,

  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 10,
  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.num_accumulated_range_data = 4
TRAJECTORY_BUILDER_2D.use_imu_data = false

return options

• my_robot_2d_localization.launch
  作用：用实际机器人定位

• my_robot_2d.launch
  作用：用实际机器人建图

• offline_my_robot_2d.launch
  作用：用离线的rosbag数据包建图（仅有激光雷达数据）
  参考启动方式：

roslaunch cartographer_ros offline_my_robot_2d.launch bag_filenames:=~/ws_ros/cartographer/bag_data/b2-2016-04-05-14-44-52.bag