ROS学习（十九）机器人SLAM功能包——cartographer

---

前言

  cartographer的核心是cartographer_node节点，当运行该节点时，需要一个Lua编写的.lua文件，该文件的主要作用是进行参数配置，这与gmapping、hector在launch文件中直接配置参数不同。
  cartographer的亮点在于代码规范与工程化，适合商业应用和二次开发，能有效避免建图过程中环境中移动物体的干扰，支持多传感器数据建图，支持2D-slam和3D-slam建图。
  之前我自定义过一个机器人模型，后来在使用cartographer过程中，感觉有问题，但是这个问题我解决不了，不知道是配置的问题，还是TF的问题，哈哈。所以在本篇开始，舍弃之前自定义的机器人模型，重新自定义一个机器人模型，即使用赵老师的模型，也方便后续kinect的使用。该模型的xacro可以去网上百度下，在这里不再介绍。

一、cartographer_2d.launch文件

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>
  <param name="/use_sim_time" value="true" />

  <node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" args=" -configuration_directory $(find robot_slam_cartographer)/configuration_files -configuration_basename rplidar.lua" output="screen">
    <remap from="scan" to="scan" />
    <param name="base_link" value="base_footprint"/>
  </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 robot_slam_cartographer)/config/default.rviz" />

</launch>

该文件主要包含两部分工作，一是运行cartographer_node节点，二是启动rviz可视化界面。当运行cartographer_node节点时，需要用到一个由lua编写的代码文件rplidar.lua，进行参数配置，lua文件内容后面介绍。

二、mbot_sim_gazebo_navigation_world.launch文件

launch文件内容如下：

<?xml version="1.0"?>
<launch>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="worlds/mbot_sim_gazebo_navigation.world"/> 
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>
</launch>

三、robot_gazebo_navigation.launch文件

launch文件内容如下：

<?xml version="1.0"?>
<launch>
 	<!-- 运行gazebo仿真环境 -->
    <include file="$(find gazebo_ros)/launch/mbot_sim_gazebo_navigation_world.launch">
    </include>

    <!--机器人参数设置-->
    <arg name="model" default="$(find xacro)/xacro --inorder '$(find robot_slam_cartographer)/urdf/xacro/robot.xacro'" />
     
    <!--robot_description 参数名称是参数服务器预先设定好的，不能改变-->
    <param name="robot_description" command="$(arg model)" />
    
    <!-- 运行joint_state_publisher节点，发布机器人的关节状态 -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

    <!-- 运行robot_state_publisher节点，发布tf -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

      <!-- 在gazebo中加载机器人模型-->
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -model mrobot -param robot_description"/> 

    <!--<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_slam_cartographer)/config/default.rviz"/>-->

    <!--gmapping 包实现同步定位和地图建立 <node name="slam_gmapping" pkg="gmapping" type="slam_gmapping"> <remap from="scan" to="/scan"/> <param name="base_link" value="base_footprint"/> </node>-->

    <include file="$(find robot_slam_cartographer)/launch/cartographer_2d.launch"/>

</launch>

四、rplidar.lua文件

文件内容如下：

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

options = {
    
    map_builder = MAP_BUILDER,
    trajectory_builder = TRAJECTORY_BUILDER,
    map_frame = "map",
    tracking_frame = "base_link",
    published_frame = "base_link",
    odom_frame = "odom",
    provide_odom_frame = true,
    use_odometry = 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,


}

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

SPARSE_POSE_GRAPH.optimization_problem.huber_scale = 1e2
SPARSE_POSE_GRAPH.optimize_every_n_scans = 35
SPARSE_POSE_GRAPH.constraint_builder.min_score = 0.65     

return options

五、编译运行

每一次修改lua文件后，都需要编译，命令如下：

catkin_make_isolated --install --use-ninja

运行launch文件，命令如下：

roslaunch robot_slam_cartographer robot_gazebo_navigation.launch

然后，再运行键盘控制启动文件，命令如下：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

最后，建图效果如下：

六、保存地图

方法1、使用map_saver直接保存为pgm和yaml文件

切换到map目录，新建map_saver_map文件夹，执行如下命令：

rosrun map_server map_saver -f myMapFile

方法2、先保存为.pbstream文件，再转化为pgm和yaml文件

步骤如下：

1、停止地图构建

rosservice call /finish_trajectory 0

2、生成.pbstream文件

rosservice call /write_state "{filename: '${HOME}/catkin_google_ws/src/robot_slam_cartographer/map/carto_map.pbstream'}"

3、将pbstream转化为pgm和yaml文件

rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=${
    HOME}/catkin_google_ws/src/robot_slam_cartographer/map/myMapFile -pbstream_filename=${
    HOME}/catkin_google_ws/src/robot_slam_cartographer/map/carto_map.pbstream -resolution=0.05

执行效果如下：

地图在功能包中位置如下：