ubuntu20.04下turtlebot3与cartographer结合使用–建图与定位

ubuntu20.04下turtlebot3与cartographer结合使用–建图与定位

---

参考链接

turtlebot3安装

• turtlebot3

• turtlebot3_msgs

• turtlebot3_simulations

  以上3个功能包都需安装编译

Cartographer安装

GitHub

• https://github.com/cartographer-project/cartographer
• https://github.com/cartographer-project/cartographer_ros

Google文档–强力推荐

• Cartographer ROS

CSDN

• cartographer探秘第一章之安装编译与参数配置

---

2D建图仿真

1.启动Gazebo仿真

roslaunch turtlebot3_gazebo turtlebot3_stage_4.launch

2.启动slam

turtlebot3已包含与cartographer相关的启动配置文件，故只需安装好cartographer，便可通过以下命令启动cartographer的slam建图，包含move_base节点

roslaucn turtlebot3_slam turtlebot3_slam.launch slam_methods:=cartographer

注：
turtlebot3_slam/config文件目录下已有turtlebot3_lds_2d.lua，也可以自定义新建.lua文件
同样保存在turtlebot3_slam/config文件目录下，方便后续动态调用

参考古月居的文档
turtlebot3_carto.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

通过以下命令动态加载自定义.lua文件

roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:=cartographer configuration_basename:=turtlebot3_carto.lua

3. 将地图保存.pbstream文件

参考链接

rosservice call /write_state "{filename: '/home/chj/maps/carto_test.pbstream', include_unfinished_submaps: true}"

filename路径自行选择

4. 可以通过cartographer提供的ros包，将后缀名为.pbstream的地图信息，转化成ros地图信息，用于其他方式的定位，如AMCL

rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=/home/chj/maps/carto_test -pbstream_filename=/home/chj/maps/carto_test.pbstream -resolution=0.05

5. 基于已有地图定位

使用已经建立好的地图进行定位，同时进行增量式更新地图

• 在turtlebot3_slam/config文件目录下新建
  turtlebot3_carto_localization.lua

include "turtlebot3_carto.lua"

--TRAJECTORY_BUILDER.pure_localization = true (不需要这句，否则报错)
TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 50

return options

• 选择在turtlebot3_slam/launch文件目录下新建turtlebot3_carto_localization.launch

<launch>
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="configuration_basename" default="turtlebot3_carto_localization.lua"/>
  <arg name="open_rviz" default="true"/>
  <arg name="arg load_state_filename" default="/home/chj/maps/carto_test.pbstream"/>

  <!-- TurtleBot3 -->
  <include file="$(find turtlebot3_bringup)/launch/turtlebot3_remote.launch">
    <arg name="model" value="$(arg model)" />
  </include>

  <!-- move_base -->
  <include file="$(find turtlebot3_navigation)/launch/move_base.launch">
    <arg name="model" value="$(arg model)" />
  </include>

  <!-- cartographer_node -->
  <node pkg="cartographer_ros" type="cartographer_node" name="cartographer_node" 
        args="-configuration_directory $(find turtlebot3_slam)/config
              -configuration_basename $(arg configuration_basename)
              -load_state_filename $(arg load_state_filename)"
        output="screen">
    <remap from="/imu" to="/flat_imu"/>
    <!--remap from="points2" to="/camera/depth/points" / -->
  </node>

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

  <!-- flat_world_imu_node -->
  <node pkg="turtlebot3_slam" type="flat_world_imu_node" name="flat_world_imu_node" output="screen">
    <remap from="imu_in" to="/imu" />
    <remap from="imu_out" to="/flat_imu" />
  </node>

  <!-- rviz -->
  <group if="$(arg open_rviz)"> 
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find turtlebot3_slam)/rviz/turtlebot3_$(arg slam_methods).rviz"/>
  </group>
</launch>

注：
动态参数load_state_filename的default应当为之前保存的.pbstream路径

• 仿真测试

roslaunch turtlebot3_gazebo turtlebot3_stage_4.launch

roslaunch turtlebot3_slam turtlebot3_carto_localization.launch