第四讲 【cartographer】纯定位 纯本地化 pure_localization

系列文章目录

第一讲【ROS-SLAM】2D激光雷达 cartographer构建地图

第二讲 【cartographer】Ubuntu16.04 kinetic 最新版cartographer安装（2020/11/4更新）

第三讲 【cartographer】 添加功能以从RVIZ为纯本地化模式设置初始姿势

第四讲 【cartographer】纯定位 纯本地化 pure_localization

第五讲【cartographer】在仿真环境中 建图 纯定位

第六讲【cartographer】纯定位参数优化（初级篇）

---

---

前言

随着5G的不断发展，无人驾驶也越来越重要，很多人都开启了学习SLAM中的cartographer，本文就介绍了cartographer纯定位模式，也就是纯本地化，也叫pure_localization。

---

一、cartographer是什么？

cartographer算法的翻译叫做制图师，实际的作用就是SLAM。SLAM (simultaneous localization and mapping),也称为CML (Concurrent Mapping and Localization), 即时定位与地图构建，或并发建图与定位。
也就是说cartographer有建图与定位的功能。下面就是介绍cartographer定位模式。

二、cartographer纯定位步骤

1.引入launch启动文件

demo_backpack_2d_localization.launch

<launch>
  <!-- <param name="/use_sim_time" value="true" /> -->
  <param name="/use_sim_time" value="false" />

  <!-- <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" /> -->

  <!-- <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" /> -->

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename backpack_2d_localization.lua
          -start_trajectory_with_default_topics = false
          -load_state_filename $(find lyric_maps_manager)/scripts/map/mymap.pbstream" 
      output="screen">
    <remap from="echoes" to="scan" />
    <remap from="imu" to="imu_data" />
    <remap from="odom" to="odom_tf" />
  </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" />
  <node name="playbag" pkg="rosbag" type="play"
      args="clock $(arg bag_filename)" /> -->
</launch>

backpack_2d_localization.lua

include "revo_lds_2d_localization.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 5,
}
POSE_GRAPH.optimize_every_n_nodes = 20

return options

2.lua参数文件

backpack_2d_localization.lua

include "revo_lds_2d_localization.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 5,
}
POSE_GRAPH.optimize_every_n_nodes = 20

return options

revo_lds_2d_localization.lua

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_link",
  published_frame = "odom_combined",
  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.5,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 2e-2,
  --启用将跟踪的姿势发布为geometry_msgs / PoseStamped到主题“ tracked_pose”。
  --publish_tracked_pose_msg = true,
  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.,
  }
  --pure_localization
  POSE_GRAPH.optimize_every_n_nodes = 0
  MAP_BUILDER.num_background_threads = 4
  POSE_GRAPH.global_sampling_ratio = 0.003
  POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
  POSE_GRAPH.constraint_builder.min_score = 0.85
  POSE_GRAPH.global_constraint_search_after_n_seconds = 30
  --pure_localization end
  -- POSE_GRAPH.optimization_problem.log_solver_summary = true

  MAP_BUILDER.use_trajectory_builder_2d = true
  
  TRAJECTORY_BUILDER_2D.submaps.num_range_data = 45
  TRAJECTORY_BUILDER_2D.min_range = 0.05
  TRAJECTORY_BUILDER_2D.max_range = 15.
  TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
  TRAJECTORY_BUILDER_2D.use_imu_data = true
  --重力常数
  TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 9.7883
  
  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.angular_search_window = math.rad(45.)
  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

  --扫描匹配器可以在不影响分数的情况下自由地前后移动匹配项。我们希望通过使扫描匹配器支付更多费用来偏离这种情况，
  --从而对这种情况进行惩罚。控制它的两个参数是TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight和
  --rotation_weight。越高，将结果从先前移开，换句话说，就越昂贵：扫描匹配必须在要接受的另一个位置产生更高的分数。
  --TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 2e2
  --TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 4e2


  POSE_GRAPH.optimization_problem.huber_scale = 1e2
  --odom
  --POSE_GRAPH.optimization_problem.initial_pose_translation_weight = 1e5
  --POSE_GRAPH.optimization_problem.initial_pose_rotation_weight = 1e5
  --POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e5
  --POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e1
  

return options

---

总结

不想长篇大论，干脆直接给出参数启动文件，正常情况下，安装正确的cartographer之后，只需要将imu，odom的话题remap到正确的话题，使用上面的参数启动文件，是可以实现定位的。

tips：
如果不想一边定位，地图一边变化，需要注释掉下面这行代码。

本博主在仿真环境中使用最新版本的cartographer_ros的时候，注释以下代码，也不能实现地图不更新。
可能原因：
1、最新版本不可以通过该操作实现地图不更新。（已排除该原因）

2、第一次能够实现是因为进行了其他的代码修改操作。（已排除该原因）

 // occupancy_grid_publisher_.publish(*msg_ptr);