上一节我们介绍了wpr_simulation这个开源项目的简单使用，这一节我们继续深入，在Gazebo里进行SLAM建图和Navigation导航的仿真。
一、代码更新
项目wpr_simulation的代码为这次实验进行了更新，需要重新下载。如果之前有下载了老版本的代码，可以进入~/catkin_ws/src，直接删除wpr_simulation文件夹，然后按照下面操作下载新的代码。
从Ubuntu桌面左侧的启动栏里点击“Terminal”终端图标，启动终端程序（也可以通过同时按下键盘组合键“Ctrl+Alt+T”来启动），输入如下指令下载项目源码：
其中“catkin_ws”为ROS的工作空间，请根据电脑的环境设置进行修改。这个项目的源码是从Github网站下载的，所以下载过程需要连接互联网。
然后是启智ROS机器人的源码包wpb_home，这个项目如果之前已经下载过，这次不用更新，使用以前的代码也可以。
所有源码下载完成后，运行如下指令进行源码工程的编译：
这个开源工程，用到了Gazebo，如果之前安装的ROS是完整版本，则会很顺利的编译通过。如果编译过程中提示缺少某些依赖项，可以通过如下指令补充完整：
二、SLAM原理简介
在进行具体实验之前，我们先简单介绍一下原理。“SLAM”，英文全称是“SimultaneousLocalizationAndMapping”，翻译过来就是“即时定位与地图构建”。SLAM最早由Smith、Self和Cheeseman于年提出，由于其重要的理论与应用价值，被很多学者认为是实现真正全自主移动机器人的关键。要理解SLAM，先得理解激光雷达的数据特点，激光雷达的扫描数据可以理解为一个障碍物分布的切面图，其反映的是在一个特定高度上，障碍物面向雷达的面的边缘形状和分布位置。
所以，当携带激光雷达的机器人在环境中运动时，它在某一个时刻，只能得到有限范围内的障碍物的部分轮廓和其在机器人本体坐标系里的相对位置。比如在下图中，反映了一个机器人在相邻比较近得A、B、C三个位置激光雷达扫描到的障碍物轮廓。
虽然此时我们还不知道位置A、B、C的相互关系，但是通过仔细观察，可以发现在A、B、C三个位置所扫描到的障碍物轮廓，某一些部分，是可以匹配重合的。因为这三个位置离得比较近，我们就假设扫描到的障碍物轮廓的相似部分就是同一个障碍物，这样就可以试着将相似部分的障碍物轮廓叠加重合在一起，得到一个更大的障碍物轮廓图案。比如位置A和位置B的障碍物轮廓叠加后如下：
再比如，位置B和位置C的障碍物轮廓叠加后如下：
按照上述的方法，将连续的多个位置激光雷达扫描到的障碍物轮廓拼合在一起，就能形成一个比较完整的平面地图。这个地图是一个二维平面上的地图，其反映的是在激光雷达的扫描面上，整个环境里的障碍物轮廓和分布情况。在构建地图的过程中，还可以根据障碍物轮廓的重合关系，反推出机器人所走过的这几个位置之间的相互关系以及机器人在地图中所处的位置，这就同时完成了地图构建和机器人的自身实时定位这两项功能，这也就是“SLAM”全称“SimultaneousLocalizationAndMapping”的由来。同样以前面的A、B、C三个位置为例，将三个位置的激光雷达扫描轮廓拼合在一起，就能得到一个相对更完整的平面地图，同时也得出A、B、C三个位置在这个地图中的位置：
ROS支持多种SLAM算法，其中主流的是HectorSLAM和Gmapping，其中HectorSLAM仅依靠激光雷达就能工作，其原理和前面描述的方法比较类似；Gmapping则是在上述方法的基础上，还融合了电机码盘里程计等信息，其建图的稳定性要高于HectorSLAM。在这一节实验里，我们主要使用Gmapping。
三、SLAM建图的仿真
我们先进行SLAM建图的仿真。在这个仿真中，我们将使用Gmapping算法，需要使用USB手柄来控制机器人在场景中进行移动，遍历所有活动区域，建立环境地图。
将手柄接到电脑USB上之后，通过如下指令启动SLAM建图的仿真场景
启动后，会弹出Gazebo窗口，里面显示的是一个模拟RoboCup
Home比赛环境的场景。场景中共有四个房间，分别为客厅、卧室、餐厅和厨房，在每个房间里都放置了一些床、柜子之类的家具。这个场景还有两个出入口，我们的机器人初始位置就位于靠近客厅的这个入口。我们建图的效果，主要是在Rviz里进行观察。在Ubuntu的左侧任务栏里，可以看到Rviz的程序图标，用鼠标点击将Rviz界面唤回前台显示。
在Rviz里，可以看到机器人面前大片的地面基准都是深灰色，只有机器人脚下出现一片白色图案。这个图案，是由很多条线段叠加而成，这些线段是机器人本体中心地面投影和每一个激光雷达红色障碍点的连线，也就是测距激光飞行的轨迹，表示这条线段内部没有障碍物。
我们可以使用USB手柄遥控机器人移动，在场景里巡游，Gmapping会调用内部的SLAM算法，把整个场景的地图都扫描出来。
如果没有USB手柄怎么办？没关系，这里准备个程序，可以使用键盘控制机器人完成巡游移动。在Ubuntu里再打开一个终端，输入如下指令：
回车执行后，会提示控制机器人移动对应的按键列表。需要注意的是，在控制过程中，必须让这个终端程序位于Gazebo窗口前面，处于选中状态。这样才能让这个终端程序能够持续获得按键信号。
用键盘控制机器人移动的时候，只需要点一下键盘按键就可以让机器人沿着对应方向移动，不需要一直按着不放，必要的时候使用空格键刹车。机器人在场景里巡游一遍之后，可以看到建好的地图如下：
我们把地图保存下来，后面进行自主导航时会用到。保存地图时，需要保持Gmapping的程序仍在运行，不能关闭建好图的Rviz界面。启动一个终端程序，输入以下指令：
按下回车键，确认保存。
保存完毕后，会在Ubuntu系统的“主文件目录”里生成两个新文件，一个名为“map.pgm”，另一个名为“map.yaml”，这就是我们使用Gmapping建好的环境地图。现在我们可以关闭Gazebo和Rviz窗口，准备进行导航的仿真。
SLAM建图仿真视频
四、Navigation导航仿真
首先，将“主文件目录”里的“map.pgm”和“map.yaml”两个文件都拷贝到工作目录“catkin_ws/src/wpr_simulation/maps”里:
这个复制操作也可以在“文件管理器”里用鼠标完成：
地图文件放置完毕，输入以下指令启动导航仿真：
按下回车键，系统会启动Gazebo窗口，可以看到机器人又回到初始的那个入口。
在Ubuntu的左侧任务栏里，可以看到Rviz的程序图标，用鼠标点击将Rviz界面唤回前台显示。
在Rviz窗口中，可以看到机器人位于我们刚才建好的地图当中。
再仔细看看此时的地图，在原来的黑色图案（静态障碍物轮廓）的周围，出现了蓝色的色带，这个色带表示的是安全边界，色带宽度和机器人的半径大致相等。也就是说如果机器人进入这个色带，就有可能和静态障碍物（墙壁或桌椅腿）发生碰撞，这个安全边界在后面机器人规划路径时会用到。
另外一个可以注意到的，Rviz中机器人的当前位置在地图中央，这个和实际机器人位置不符。我们需要在导航前将机器人设置到正确的位置，点击Rviz界面上方工具栏条里的“2DPosEstimate”按钮：
然后再点击Rviz的地图里，现实机器人所处的位置。这时，会出现一个绿色大箭头，代表的是机器人在初始位置的朝向。按住鼠标键不放，在屏幕上拖动画圈，可以控制绿色箭头的朝向。
在Rviz中拖动绿色箭头，选择好朝向，松开按键，机器人模型就会定位到我们选择的位置。
调整虚拟机器人的初始位置，直到红色的激光雷达数据点和静态障碍物的轮廓大致贴合。设置好机器人的初始位置后，可以开始为机器人指定导航的目标地点。点击Rviz界面上方工具条里的“2DNavGoal”按钮：
然后点击Rviz里地图上的导航目标点（通常在白色区域里选择一个地点）。此时会再次出现绿色箭头，和前面的操作一样，按住不放在屏幕上拖动画圈，设置机器人移动到终点后的朝向。
选择完目标朝向后，松开点击，全局规划器会自动规划出一条紫色的路径，这条路径从机器人当前点出发，绕开蓝色的安全边界，一直到移动目标点结束。
路径规划完毕后，机器人会开始沿着这条路径移动，此时切换到Gazebo窗口，可以看到仿真环境里的机器人也开始沿着这条路径移动。
机器人到终点后，会原地旋转，调整航向角，最终朝向刚才设置目标点时绿色箭头的方向。
Navigation导航仿真视频
五、用代码控制机器人进行导航
ROS中的Navigation导航过程是可以通过代码来控制的，这里有个简单的导航程序可以供我们参考，它的源代码位置：
我们可以用VisualStudioCode之类的IDE打开这个源码文件：
(1)代码的开始部分，先include了三个头文件，第一个是ros的系统头文件，第二个是导航目标结构体move_base_msgs::MoveBaseGoal的定义文件，第三个是actionlib::SimpleActionClient的定义文件。
(2)ROS节点的主体函数是intmain(intargc,char**argv)，其参数定义和其他C++程序一样。
(3)main函数里，首先调用ros::init(argc,argv,demo_simple_goal);进行该节点的初始化操作，函数的第三个参数是节点名称。
(4)接下来声明一个MoveBaseClient对象ac，用来调用和主管监控导航功能的服务。
(5)在请求导航服务前，需要确认导航服务已经开启，所以这里调用ac.waitForServer()函数来查询导航服务的状态。ros::Duration()是睡眠函数，参数的单位为秒，表示睡眠一段时间，这段时间若被某个信号打断（在这个例程里，这个信号就是导航服务已经启动的信号），则中断睡眠。所以ac.waitForServer(ros::Duration(5.0));的意思就是：休眠5秒，若期间导航服务启动了，则中断睡眠，开始后面的操作。用while循环来嵌套，可以让程序在休眠5秒后，若导航服务未启动，则继续进入下一个5秒睡眠，直到导航服务启动才中断睡眠。
(6)确认导航服务启动后，我们声明一个move_base_msgs::MoveBaseGoal类型结构体对象goal，用来传递我们要导航去的目标信息。goal.target_pose.header.frame_id表示这个目标位置的坐标是基于哪个坐标系，例程里赋值“map”表示这是一个基于全局地图的导航目标位置。goal.target_pose.header.stamp赋值当前时间戳。goal.target_pose.pose.position.x赋值-3.0，表示本次导航的目的地是以地图坐标系为基础，向X轴反方向移动3.0米。goal.target_pose.pose.position的y赋值2.0，表示本次导航的目的地是以地图坐标系为基础，向Y轴正方向移动2.0米。goal.target_pose.pose.position的z未赋值，则默认是0；goal.target_pose.pose.orientation.w赋值1.0，则导航的目标姿态是机器人面朝X轴的正方向（正前方）。
(7)ac.sendGoal(goal);将导航目标信息传递给导航服务的客户端ac，由ac来监控后面的导航过程。
(8)ac.waitForResult();等待MoveBase的导航结果，这个函数会保持阻塞，就是卡在这，直到整个导航过程结束，或者导航过程被其他原因中断。
(9)ac.waitForResult()阻塞结束后，调用ac.getState()获取导航服务的结果，如果是“SUCCEEDED”说明导航顺利到达目的地，输出结果“Mission