使用cartographer纯定位替换amcl定位

前言：经过实际使用，发现cartographer定位对于重定位的表现比amcl好。如果机器人的起始位置离原点较远，amcl很难自行重定位到正确的位置，只能手动修改初始位置。然而cartographer定位的表现则很好，在行驶一段距离或原地旋转一段时间后能够正确重定位。

一、保存cartographer建的地图

1、在建图完毕后，使用cartographer自带的命令保存地图，此处可以修改存图的目录

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

2、将pbstream格式的地图转换为pgm和yaml格式，以备后用。也可以使用map_server保存地图

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

二、cartographer定位的配置文件

配置3个文件，以下文件名是自定义的，可以直接在相应的目录新建一个文件，把内容复制进去

1、carto_localization.launch文件
其中scan和imu_data需要替换为自己的话题名字，map.pbstream文件的位置也要修改为自己的

<launch>
  <param name="/use_sim_time" value="false" />
  <!--start the graph node-->
  <node name="cartographer_node_2" pkg="cartographer_ros" type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename pure_localization.lua
          -load_state_filename /home/robuster/navigation_ws/src/nav_manage/map/map.pbstream"
      output="screen">
      
    <remap from="scan" to="scan" />
    <remap from="points2" to="rslidar_points" />
    <remap from="imu" to="imu_data" />
  </node>
</launch>

2、pure_localization.lua文件

include "carto_localization.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 80  --每80个有效帧组成一个子图，子图构建完成要闭环检测一次，这个数越小，闭环检测越频繁

return options

3、carto_localization.lua文件
注意：此处的provide_odom_frame参数会影响tf树中的odom节点。如果你机器人上的里程计精度不高，就可以将此参数置为true，也就是使用cartographer的激光里程计。这种情况下切记要关闭原本的odom -> base_link的tf转换，否则会有tf树冲突。

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

options = {
  map_builder = MAP_BUILDER,                -- map_builder.lua的配置信息
  trajectory_builder = TRAJECTORY_BUILDER,  -- trajectory_builder.lua的配置信息
  
  map_frame = "map",                        -- 地图坐标系的名字
  tracking_frame = "imu_link",              -- 此处需要是imu的坐标系，否则会报错
  published_frame = "base_link",            -- tf: map -> base_link
  odom_frame = "odom_link",                 -- 里程计的坐标系名字
  provide_odom_frame = true,                -- 是否提供odom的tf, 如果为true,则tf树为map->odom->base_link
                                            -- 如果为false tf树为map->base_link
  publish_frame_projected_to_2d = false,    -- 是否将坐标系投影到平面上
  --use_pose_extrapolator = false,          -- 发布tf时是使用pose_extrapolator的位姿还是前端计算出来的位姿

  use_odometry = false,                     -- 是否使用里程计,如果使用要求一定要有odom的tf
  use_nav_sat = false,                      -- 是否使用gps
  use_landmarks = false,                    -- 是否使用landmark
  num_laser_scans = 0,                      -- 是否使用单线激光数据
  num_multi_echo_laser_scans = 0,           -- 是否使用multi_echo_laser_scans数据
  num_subdivisions_per_laser_scan = 1,      -- 1帧数据被分成几次处理,一般为1
  num_point_clouds = 1,                     -- 是否使用点云数据
  
  lookup_transform_timeout_sec = 0.2,       -- 查找tf时的超时时间
  submap_publish_period_sec = 0.3,          -- 发布数据的时间间隔
  pose_publish_period_sec = 0.1,            -- 发布tf坐标转换的频率
  trajectory_publish_period_sec = 0.1,

  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.use_imu_data = true
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 100.
TRAJECTORY_BUILDER_2D.min_z = 0.2

TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 1.

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80.
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.1

POSE_GRAPH.optimize_every_n_nodes = 160.
POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
POSE_GRAPH.constraint_builder.min_score = 0.48
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60

return options

三、使用cartographer定位 发布实时的全局位置信息

1、找到cartographer_ros包中的node.cc文件，查找里面的publish_tracked_pose，可以看到这个变量决定是否发布定位信息。转到publish_tracked_pose的定义处，将其修改为true

bool publish_tracked_pose = true;

2、运行上面的carto_localization.launch文件，查询所有话题可以发现，出现了一个tracked_pose话题，这个就是cartographer节点发布的全局定位信息。

3、 用cartographer替换amcl的定位
amcl_pose的数据类型是PoseWithCovarianceStamped，tracked_pose的数据类型是PoseStamped，这两个数据类型都有x,y,z和四元数的信息。因此可以直接将订阅amcl_pose的代码替换为订阅tracked_pose。

订阅amcl_pose的代码

void DWAPlanner::amclCallBack(const geometry_msgs::PoseWithCovarianceStamped& amcl_msg)
{
    double cur_pose_x = amcl_msg.pose.pose.position.x;
    double cur_pose_y = amcl_msg.pose.pose.position.y;

    double x = amcl_msg.pose.pose.orientation.x;
    double y = amcl_msg.pose.pose.orientation.y;
    double z = amcl_msg.pose.pose.orientation.z;
    double w = amcl_msg.pose.pose.orientation.w;
    double siny_cosp = 2 * (w * z + x * y);
    double cosy_cosp = 1 - 2 * (y * y + z * z);
    double cur_pose_theta = std::atan2(siny_cosp, cosy_cosp);     //  rad/s

    cur_pose_[0] = cur_pose_x;
    cur_pose_[1] = cur_pose_y;
    cur_pose_[2] = cur_pose_theta;
}

替换为订阅tracked_pose

void DWAPlanner::cartoCallBack(const geometry_msgs::PoseStamped& carto_msg)
{
    double cur_pose_x = carto_msg.pose.position.x;
    double cur_pose_y = carto_msg.pose.position.y;

    double x = carto_msg.pose.orientation.x;
    double y = carto_msg.pose.orientation.y;
    double z = carto_msg.pose.orientation.z;
    double w = carto_msg.pose.orientation.w;
    double siny_cosp = 2 * (w * z + x * y);
    double cosy_cosp = 1 - 2 * (y * y + z * z);
    double cur_pose_theta = std::atan2(siny_cosp, cosy_cosp);     //  rad/s

    cur_pose_[0] = cur_pose_x;
    cur_pose_[1] = cur_pose_y;
    cur_pose_[2] = cur_pose_theta;
}