写在前面

上一小节我们完成了数据包的构建，并且实现了对基础硬件的控制。从这一小节开始，我们将进入基于ROS的上位机程序设计。本小节主要介绍了：

• 三维空间测绘机器人的ROS节点分布
• 启动文件设计
• 上位机中的通讯（下位机驱动）程序

上位机中节点的分布

上位机功能模块概述

我们上位机整体将基于ROS进行设计，对ROS基础不是十分了解的亲们可以去http://wiki.ros.org进行学习。 在我们设计的上位机软件架构中，将包含如下几个节点模块：

• 硬件驱动模块，包含：
  1. 激光雷达驱动节点，用于获取2D激光扫描数据
  2. 双目摄像头驱动节点，用于视觉校准以及对点云数据进行上色
  3. 下位机驱动节点（mcu_driver），用于驱动机器人移动以及舵机z轴扫描
• 基础导航模块（move_base），用于对机器人进行初步定位，生成简易二维地图
• 点云生成模块，包含：
  1. 激光雷达数据预处理节点
  2. 激光反射点空间位置解算节点
  3. 激光点云生成节点
• 点云处理及导航节点
  1. 点云滤波处理节点
  2. 点云导航定位节点
  3. 上层应用（提供二次开发的接口）

这些模块的整体架构如下图所示：    

上位机的软件架构及launch文件设计

在上面的图中，我们把最底下的一层称作硬件驱动节点。 我们的硬件驱动节点包括以下三个硬件驱动

• mcu_driver，开启后与下位机进行通讯，mcu_driver的接口如下：
  1. 接收/cmd_vel，类型为Twist的消息（速度控制）
  2. 接收/led,类型为UInt8的消息（灯光控制）
  3. 提供/get_battery_state服务，用于获取机器人当前电量
  4. 提供服务，用于机器人的头部（二轴云台旋转）
• cam_driver，开启后读取USB双目摄像头的图像，并通过TCP-jpeg编码流发布
• ylidar_driver，开启后读取USB连接的雷达，并生成名为/scan，类型为LaserScan数据。

这三个ROS节点我都已经写好了（下图中红框框出的文件夹），有想交流的的可以文末小窗我。     所以，我们首先在工作空间中编写一个负责硬件驱动的launch文件，一次性将这三个节点都启动起来：  

2D导航之gmapping的使用

在进行三维测绘之前，我们必须要让机器人知道它在一个未知环境中的平面位置，依托于一个2D的平面位置（下图中的x，y），我们不断地扫描z轴，这样才能进行3D测绘。    

mcu_driver与ylidar_driver输出TF与激光雷达信息

gmapping是ROS框架自带的一个SLAM开源包，我们可以通过以下指令下载： sudo apt-get install ros-[版本号]-gmapping 我们可以通过一个指令直接运行gmapping,但是这种运行没有意义。要想gmapping顺利运行，我们需要两样东西:

• 激光雷达数据
• 从激光雷达数据到/odom坐标系的空间TF变换

gmapping的工作原理如下： 当机器人运动时，我们可以通过两个轮子上的里程计积分推测出机器人的里程和位置（称之为航迹推演），但是会有很大的漂移（见下图），下图中机器人走了红线框出的“口”字，但是在软件上呈现出来的轨迹就变得扭曲了。     gmapping在绘制地图的过程中，使用了雷达等传感器的反馈数据，对里程进行实时修正，同时形成地图。而这，也正是SLAM（Simultaneously Localization and Mapping）的核心思想。 gmapping使用了坐标系（TF）发布的方式实现了。因为机器人的位置与实际偏差很大，所以gmapping基于机器人的传感器数据修正了原点。 我们把机器人的起始位置坐标系称为/odom。把机器人的位置坐标称为/base_link。那么由起始位置到机器人当前位置的坐标变换为： odom------>base_link 但是这个坐标系的变换是由较大误差的（参考下面的图），所以基于传感器数据，gmapping生成了一个/map坐标系，gmapping生成了一个由/map向/odom的映射，从而修正了/base_link与地图之间的关系。    

初步导航包gmapping的参数配置和使用

为了正确使用gmapping这个软件包，我们需要给它配置两个内容：

• 合理的输入信息，从odom--->laser的TF变换以及/scan数据
• 应用于gmapping的配置信息，比如，滤波器的相关参数等等......
• 启动gmapping节点。

  为了方便对gmapping进行整合，我们使用param配置文件，这类文件一般以yaml作为结尾。我们新建一个yaml文件，在其中写入相关参数（如下图）关于详细的配置可以查阅网址http://wiki.ros.org/gmapping：     配置好了以后，我们用一个launch文件启动gmapping，如下文件：   完成了配置以后，我们首先启动硬件相关的节点，包括mcu_driver以及ylidar两个节点，他们分别发布TF信息以及scan激光雷达扫描信息。   这两个节点直接启动后，再启动launch文件就可以对机器人进行导航了，导航生成的地图如下图：  

本章总结

本节我们介绍了上位机的初步软件架构，节点构成，简单介绍了slam-gmapping节点的配置和使用。 基于本小节所述的框架内容就已经可以完成一台初步的2D平面SLAM机器人了。我们下一节就将会在本节的基础上，通过云台的Z轴变换，实现由2D基础地图绘制，到空间3D点云的生成。 让我们拭目以待~

联系作者