Turtlebot2 + RplidarA2（二）Cartographer手动建图

注：前文链接：Turtlebot2 + RplidarA2（一）Gmapping手动建图，本篇实验内容基于一个新的工作空间。

1.安装rplidar_ros雷达驱动

打开终端执行如下命令，在新的工作空间内下载并编译rplidar_ros：

$ mkdir -p ~/slam_ws/src
$ cd ~/slam_ws/src
$ git clone https://github.com/robopeak/rplidar_ros
$ cd …
$ catkin_make

2.安装cartographer及其ROS集成

Cartographer是一个跨多个平台和传感器配置提供2D和3D实时同步定位和建图（SLAM）的系统，由谷歌开发，其建图效果较好。

该步骤主要参考官方教程下载、编译Cartographer ROS相关功能包，包含3个：ceres_solver、cartographer、cartographer_ros。需要的基础系统环境同上篇文章。

2.1安装并配置wstool和rosdep

$ sudo apt-get update
$ sudo apt-get install -y python-wstool python-rosdep ninja-build
#若此命令报错则去掉“-y”，手动按“y”同意即可。
$ cd ~/slam_ws
$ wstool init src

2.2下载并编译相关源码

$ wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall
$ wstool update -t src

2.3安装protoc

该步骤非常关键，如果按照官方教程中直接安装proto3将可能导致缺少依赖、新旧版本冲突从而报错，这也是本人在实验中一直无法正常编译cartographer相关功能包的原因。经过查询，最终按照cartographer 安装及报错文章中所述的方法成功安装protoc：

#如果有旧版本，卸载：
$ sudo apt-get remove libprotobuf-dev

运行如下命令，查找protoc的路径：

$ which protoc

若显示某路径，则说明旧版本protobuf未完全卸载，此情况下需要执行：

$ rm /usr/local/bin/protoc
#rm的具体路径以which protoc显示的为准！

至此完成旧版本protobuf的卸载，接下来安装新的protobuf。

#安装依赖库：
$ sudo apt-get install autoconf automake libtool curl make g++ unzip
$ cd protobuf
$ ./autogen.sh
$ ./configure
$ make
$ make check
$ sudo make install
$ sudo ldconfig
$ protoc --version
#若输出protobuf版本信息则安装成功
#此处安装完的protoc在/usr/local/bin/目录下，需要将该文件移至/usr/bin/目录下：
$ sudo mv /usr/local/bin/protoc /usr/bin/

附：若protoc版本错误则报错如下：

2.4安装deb依赖

$ cd slam_ws
$ sudo rosdep init
$ rosdep update
$ rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y

2.5编译安装

$ catkin_make_isolated --install --use-ninja
$ source install_isolated/setup.bash

2.6下载示例进行测试（可选）

到此步骤我们已经安装了Cartographer及其ROS集成，为了检验其是否能够成功运行，可以下载德意志博物馆的2D包系列到~/Downloads，启动演示：

#Download the 2D backpack example bag.
$ wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag
#Launch the 2D backpack demo.
$ roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag

3.安装turtlebot相关部分

前面我们完成了ROS、cartographer相关3功能包、rplidar_ros的安装。考虑到我们需要利用cartographer驱动Turtlebot2进行建图，接下来需要安装cartographer与turtlebot相关的功能包。

3.1安装cartographer_turtlebot

#下载功能包：
$ cd slam_ws
$ wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_turtlebot/master/cartographer_turtlebot.rosinstall
$ wstool update -t src
#安装依赖：
$ sudo rosdep init
$ rosdep update
$ rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y
#编译安装：
$ catkin_make_isolated --install --use-ninja
$ source install_isolated/setup.bash

3.2安装turtlebot_apps

建图需要用到turtlebot_apps中的turtlebot_navigation功能包，因此执行如下命令进行安装：

$ cd ~/slam_ws/src
$ git clone https://github.com/turtlebot/turtlebot_apps
$ cd …
$ catkin_make_isolated --install --use-ninja

添加环境变量，在~/.bashrc最后添加如下命令并保存：

source install_isolated/setup.bash

刷新配置：

$ source ~/.bashrc

3.3制作雷达驱动启动文件

首先在turtlebot_navigation导航功能包中新建雷达驱动路径，再复制rplidar_ros中的rplidar.launch启动文件到其中：

$ roscd turtlebot_navigation
$ mkdir -p laser/driver
$ sudo cp ~/slam_ws/src/rplidar_ros/launch/rplidar.launch laser/driver/rplidar_laser.launch

然后打开新路径下的rplidar_laser.launch文件进行修改：

• 将frame_id指定为laser：
  
• 增加 TF 定义：
  
  其中 args="0.0 0.0 0.18 0 0.0 0.0 根据自己的实际安装位置修改，具体参数含义查看tf - ROS中的static_transform_publisher部分：
  static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms
  注：TF单位为米

修改完成的rplidar_laser.launch文件完整代码如下：

<launch>
  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
  <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>  
  <param name="serial_baudrate"     type="int"    value="115200"/>
  <param name="frame_id"            type="string" value="laser"/>
  <param name="inverted"            type="bool"   value="false"/>
  <param name="angle_compensate"    type="bool"   value="true"/>
  node>

  <node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.0 0.0 0.18 0 0.0 0.0 base_link laser 100"/>
launch>

最后执行命令重新编译：

$ cd ~/slam_ws
$ catkin_make_isolated --install --use-ninja

4.启动cartographer手动建图

4.1编写启动文件

首先新建turtlebot_lidar.launch文件：

$ cd ~/slam_ws/src/cartographer_turtlebot/cartographer_turtlebot/launch
$ touch turtlebot_lidar.launch
$ sudo gedit turtlebot_lidar.launch

在其中写入如下代码：

<launch>
  <arg name="configuration_basename"/>

  <include file="$(find turtlebot_bringup)/launch/minimal.launch" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory
              $(find cartographer_turtlebot)/configuration_files
          -configuration_basename $(arg configuration_basename)"
      output="screen">
    <remap from="scan" to="/scan" />
  node>

  <node name="flat_world_imu_node" pkg="cartographer_turtlebot"
      type="cartographer_flat_world_imu_node" output="screen">
    <remap from="imu_in" to="/mobile_base/sensors/imu_data_raw" />
    <remap from="imu_out" to="/imu" />
  node>

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

launch>

新建turtlebot_lidar_2d.launch文件：

$ cd ~/slam_ws/src/cartographer_turtlebot/cartographer_turtlebot/launch
$ touch turtlebot_lidar_2d.launch
$ sudo gedit turtlebot_lidar_2d.launch

在其中写入如下代码：

<launch>
  <include file="$(find cartographer_turtlebot)/launch/turtlebot_lidar.launch">
    <arg name="configuration_basename" value="turtlebot_urg_lidar_2d.lua" />
  include>
launch>

4.2重新编译

$ cd ~/slam_ws
$ catkin_make_isolated --install --use-ninja

4.3cartographer建图

打开新终端，启动机器人底盘：

$ roslaunch turbot_bringup minimal.launch

打开新终端，启动Rplidar A2雷达：

$ roslaunch turtlebot_navigation rplidar_laser.launch

打开新终端，启动cartographer_turtlebot建图并开启Rviz：

$ roslaunch cartographer_turtlebot turtlebot_lidar_2d.launch

使用键盘手动控制机器人移动建图：

$ roslaunch turtlebot_teleop keyboard_teleop.launch

可以看到，cartographer算法建图时的Rviz显示明显比Gmapping更加清晰、效果更好。

4.4保存地图

cartographer建图得到的地图并不能直接通过map_server map_saver命令进行保存，而需要打开新终端进行如下操作：

#进入工作空间source
$ cd ~/slam_ws
$ source install_isolated/setup.bash
#完成轨迹，不接受进一步的数据：
$ rosservice call /finish_trajectory 0
#序列化保存其当前状态：
$ rosservice call /write_state “{filename: ‘${HOME}/Downloads/mymap.pbstream’}”

#将pbstream格式转换为pgm和yaml格式：
rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=${HOME}/map/rplidar_a2_cartographer -pbstream_filename=${HOME}/map/rplidar_a2_cartographer.pbstream -resolution=0.05

参考文献

使用Turtlebot 2和Rplidar A2实现Cartographer
Turtlebot + Rplidar A2使用Gmapping建图与导航
cartographer ros使用指南-安装
ROS与SLAM入门教程-cartographer在Turltlebot的应用3-构建地图
cartographer 安装及报错
cartographer 保存地图