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

系列文章目录

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

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

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

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

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

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

---

---

前言

在众多机器人研发人员中，总会出现没有机器人使用的情况。因此，在本文中介绍了机器人仿真环境的部署与使用，让你随时随地都可以拥有无数台机器人。（IT可以的，我们也可以，话说我们属不属于IT行业，本人就属于移动机器人行业）

---

一、ROS仿真环境是什么？

1.1、gazebo

gazebo就相当于验证算法的MATLAB？

gazebo是三维物理仿真平台，创建一个虚拟的仿真环境，创造数据。

1.2、rviz

rviz是三维可视化工具，把已有的数据可视化显示，显示数据。

二、仿真环境快速使用步骤

2.1.引入代码

https://github.com/DroidAITech/ROS-Academy-for-Beginners/tree/master

创建一个工作空间，使用catkin_make编译，编译后添加环境路径：

source ~/.bashrc

2.2.启动仿真环境

gazebo：

roslaunch robot_sim_demo robot_spawn.launch

键盘控制机器人节点：

rosrun robot_sim_demo robot_keyboard_teleop.py

三、cartographer建图以及纯定位

3.1、在线建图

roslaunch cartographer_ros demo_revo_lds.launch

demo_revo_lds.launch

<launch>
  <param name="/use_sim_time" value="true" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename revo_lds.lua"
      output="screen">
    <remap from="scan" 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>

revo_lds.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 = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  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 = 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.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

3.2、离线建图（bag建图）

offline_backpack_2d.launch
（如果bag录制了TF，tf_static,使用下面的launch文件）

<launch>
  <arg name="no_rviz" default="false"/>
  <param name="/use_sim_time" value="true" />

  <group unless="$(arg no_rviz)">
    <node name="rviz" pkg="rviz" type="rviz" required="true"
        args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />

    <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
        type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
  </group>

  <node name="cartographer_offline_node" pkg="cartographer_ros"
      required="$(arg no_rviz)"
      type="cartographer_offline_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basenames revo_lds.lua
          -bag_filenames $(arg bag_filenames)"
      output="screen">
    <remap from="scan" to="scan" />
  </node>
  <node name="playbag" pkg="rosbag" type="play"
      args="-r 2 -l==2  --clock $(arg bag_filenames)" />

</launch>

offline_backpack_2d.launch
（如果bag没有录制了TF，tf_static,使用下面的launch文件）

<launch>
  <arg name="no_rviz" default="false"/>
  <param name="/use_sim_time" value="true" />

  <group unless="$(arg no_rviz)">
    <node name="rviz" pkg="rviz" type="rviz" required="true"
        args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />

    <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
        type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
  </group>

  <node name="cartographer_offline_node" pkg="cartographer_ros"
      required="$(arg no_rviz)"
      type="cartographer_offline_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basenames revo_lds.lua
          -urdf_filenames $(find cartographer_ros)/urdf/backpack_2d.urdf
          -bag_filenames $(arg bag_filenames)"
      output="screen">
    <remap from="scan" to="scan" />
  </node>
  <node name="playbag" pkg="rosbag" type="play"
      args="-r 2 -l==2  --clock $(arg bag_filenames)" />

</launch>

revo_lds.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 = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  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 = 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.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

rosbag info xxx.bag

types:       nav_msgs/Odometry     [cd5e73d190d741a2f92e81eda573aca7]
             sensor_msgs/Imu       [6a62c6daae103f4ff57a132d6f95cec2]
             sensor_msgs/LaserScan [90c7ef2dc6895d81024acba2ac42f369]
             tf2_msgs/TFMessage    [94810edda583a504dfda3829e70d7eec]
topics:      /imu          2791 msgs    : sensor_msgs/Imu      
             /odom        13993 msgs    : nav_msgs/Odometry    
             /scan         5592 msgs    : sensor_msgs/LaserScan
             /tf          46647 msgs    : tf2_msgs/TFMessage    (2 connections)
             /tf_static       1 msg     : tf2_msgs/TFMessage

3.3、纯定位

roslaunch cartographer_ros demo_backpack_2d_localization.launch 

demo_backpack_2d_localization.launch

<launch>
  <param name="/use_sim_time" value="true" />
  <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 /root/ros_workspaces/bag/mymap.pbstream" 
      output="screen">
    <remap from="scan" to="scan" />
  </node>

  <node name="set_initpose" pkg="cartographer_ros" type="set_initpose_from_rviz" output="screen"
        args="
              -configuration_directory $(find cartographer_ros)/configuration_files
              -configuration_basename backpack_2d_localization.lua
              -load_state_filename /root/ros_workspaces/bag/mymap.pbstream" >
    <remap from="scan" to="scan" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.03" />

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>

backpack_2d_localization.lua（1.0.0）

master和1.0.0版本的cartographer，backpack_2d.lua纯定位的参数设置不一样。

include "backpack_2d.lua"

TRAJECTORY_BUILDER.pure_localization = true
POSE_GRAPH.optimize_every_n_nodes = 20

return options

backpack_2d_localization.lua（master）

include "backpack_2d.lua"

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

return options

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",
  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 = 20
  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_ros 1.0.0，在最新的master分支，纯定位的backpack_2d_localization.lua是不一样的。区别在于：

master分支下的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

---

总结

以上就是今天要讲的内容，本文介绍了ROS仿真环境的使用，而ROS仿真环境提供了大量能使我们快速便捷地处理数据的函数和方法以及算法、应用的测试。