cartographer建图与定位应用

---

前言

本文介绍cartographer在ubuntu18.04下的使用过程，作为笔记仅供参考。

---

一、安装cartographer

使用李想大佬的脚本进行安装，同时参考下面的文章
Cartographer 环境极速配置 + 2D 3D建图测试
在此感谢两位大佬的教程。

1.安装环境

文件下载参考上文链接。
下载完成后，将cartographer_install_2021-04-20压缩文件解压，执行安装脚本即可。

./auto-carto-build.sh

2.源码编译

2.1 下载

mkdir ~/carto_ws
cd carto_ws/
git clone https://github.com/xiangli0608/cartographer_detailed_comments_ws.git
cd cartographer_detailed_comments_ws/
git pull origin master

2.2 编译

cd cartographer_detailed_comments_ws/
./catkin_make.sh

至此，cartographer源码编译完成。

二、gazebo仿真2d建图

0.准备仿真环境

以mbot仿真机器人为例，不发布odom->dummy的TF变换，注意底盘的坐标系dummy和imu的坐标系imu_link，激光话题: /scan，里程计话题: /odom ，imu话题: /imu

1.编写lua文件

以2d_online.lua为例，在~/carto_ws/cartographer_detailed_comments_ws/src/cartographer_ros/cartographer_ros/configuration_files/目录下创建2d_online.lua文件，内容如下：

-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
--      http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.

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 = "dummy",
  odom_frame = "odom",
  provide_odom_frame = true,
  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.2
TRAJECTORY_BUILDER_2D.max_range = 20.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = true
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

--报错：map_by_time.h:43]Check failed: data.time ＞ std::prev(trajectory.end())-＞first
--修改map_by_time.h:43行 :CHECK_GT(data.time, std::prev(trajectory.end())->first)中的CHECK_GT改为CHECK_GE

--使用gazebo发布的odom数据，并发布TF变换，此时map->odom->dummy
--对使用imu数据 use_imu_data=true，要将tracking_frame设置为imu的坐标系；

2.编写启动文件

仿照lx_rs16_2d_outdoor.launch文件编写test_2d_laser.launch文件

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

  <!-- 启动cartographer -->
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename 2d_online.lua"
      output="screen">
   <!-- <remap from="points2" to="rslidar_points" /> -->
    <remap from="scan" to="/scan" />
    <remap from="odom" to="/odom" />
    <remap from="imu" to="/imu" />
  </node>
  <!-- 生成ros格式的地图 -->
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

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

3.建图保存

roslaunch mbot_gazebo mbot_laser_nav_gazebo_t.launch
roslaunch cartographer_ros test_2d_laser.launch

开始建图
键盘控制机器人进行建图，效果如下

保存地图(脚本中地图名字记得修改)

./finish_slam_2d.sh 

三、cartographer定位 + move_base导航

使用cartographer定位代替amcl定位。

3.1 编写启动文件

编写lua文件

include "2d_online.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
    max_submaps_to_keep = 3,
}
MAP_BUILDER.num_background_threads = 8
POSE_GRAPH.optimize_every_n_nodes = 80

return options

编写launch文件

<!--
  Copyright 2016 The Cartographer Authors

  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
-->

<launch>
  <!-- pbstream的地址与名称 -->
  <arg name="load_state_filename" default="$(env HOME)/carto_ws/map/test-1.pbstream"/>

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

<!-- 重定位用，接收/initialpose话题-->
  <param name="/localization" type="bool" value = "1"/>
  <param name="/set_inital_pose_x" type="double" value = "0"/>
  <param name="/set_inital_pose_y" type="double" value = "0"/>
  <param name="/set_inital_pose_z" type="double" value = "0.0"/>
  <param name="/set_inital_pose_ox" type="double" value = "0.0"/>
  <param name="/set_inital_pose_oy" type="double" value = "0.0"/>
  <param name="/set_inital_pose_oz" type="double" value = "0"/>
  <param name="/set_inital_pose_ow" type="double" value = "1"/>

  <!-- 启动cartographer -->
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename 2d_localization.lua
          -load_state_filename $(arg load_state_filename)"
      output="screen">
    <!-- <remap from="points2" to="rslidar_points" /> -->
    <remap from="scan" to="scan" />
    <remap from="odom" to="odom" />
    <remap from="imu" to="imu" />
  </node>

  <!-- 启动map_server -->
  
  <node name="map_server" pkg="map_server" type="map_server"
      args="$(env HOME)/carto_ws/map/test-1.yaml" /> 
  <!-- LX大佬修改过的-->
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05
                                                    -pure_localization 1" />  
  <!-- 启动rviz -->
  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />

</launch>

3.2 启动launch

#仿真环境
roslaunch mbot_gazebo mbot_laser_nav_gazebo_t.launch
#定位及加载地图
roslaunch cartographer_ros test_2d_laser_localization.launch
#导航
roslaunch mbot_navigation nav_cloister_demo_teb.launch

从TF可看到，cartographer定位已代替amcl定位。
导航效果图

---

总结

本文简单介绍了cartographer的使用包括建图和定位，没有进行具体的理论介绍与推导，也没有针对具体参数进行介绍，仅仅记录使用过程。