第十七章 添加你自己的移动机器人：第二部分
在第16章中，我们学习了如何将一个新的机器人——小龟机器人带入 ROS。包括定义话题API，构建完整的 Gazebo 模型，以及在仿真中使用底层的速度指令控制其运动等等。本章中，我们会更进一步，在仿真中让小龟机器人自主导航行进。达到这个目标所需的步骤如下：
- 对发生坐标变换的数据进行可视化处理，并检查数据的合法性
- 添加一个激光雷达
- 配置，整合导航程序栈
- 使用 rviz 定位机器人，发送导航目标
检查坐标变换信息
回忆前一章中启动小龟机器人仿真的命令：
命令中使用的 launch 文件会打开一个空的Gazebo仿真环境，然后开始对小龟机器人进行仿真。现在让我们打开 rviz 来看看仿真中的机器人状态如何。保持Gazebo 运行，打开 rviz：
你可能会怀疑Gazebo 和 rviz 究竟是不是两个独立的程序。没错，它们看起来非常相像：视图都是三维的，都允许你从不同的角度观察机器人和周边的环境等等。但它们确实是两个不同的程序，因为各自在 ROS 中的功能大不相同。Gazebo 用于对机器人进行仿真，而 rviz 则用于进行可视化。Gazebo 替代了真实物理环境中的机器人，通过计算，Gazebo 仿真了各种物理上的力学效应，还能合成传感器的数据。三维显示则只是Gazebo 中的一个可选的功能而已（虽然三维显示对于仿真而言确实很重要）。因此在类似持续集成测试（CI）这样的应用场景下，Gazebo 运行时通常是不带图形界面的。而 rviz 的任务则是从传感器处获得数据，对机器人的状态进行可视化。换言之，rviz 展示的是机器人所“认为”的当前状态（借助传感器数据计算得到），而 Gazebo 展示的则是机器人“实际”的当前状态（通过仿真得到）。
为了可视化我们的机器人，还需要对 rviz 进行一些配置（配置完成后，注意在退出 rviz 时选择保存，这样就不用在下一次打开rivz时重新配置一遍）。
- 在 Displays->Global Options 菜单中，设置“fixed frame”为“odom”。这样我们就能看到机器人相对于出发点的运动情况了。
- 在 Displays 菜单中点击 “Add” 按钮，然后选择“RobotModel”并单击“OK”。这样 rviz 就会从参数服务器中读取小龟机器人的 URDF 模型，并显示出来。
最后的效果如图 Figure17-1 所示。看起来不太对，机器人的底盘和脚轮虽然连在一起，但是相对位置是错的，而且轮子也不见了。实际上 rviz 正在报错中： Display->RobotModel 里的错误信息表明，rviz 缺少许多连接段之间的变换信息。
问题的原因在于我们没有发布坐标变换数据。和许多 ROS 工具一样，rviz 需要从/tf 话题中获取不同坐标系之间的位置关系（消息类型为 tf2_msgs/TFMessage），这些信息应当由我们提供。解决办法很简单，两步就可以完成：
- 把每个连接点的状态以 sensor_msgs/JointState 类型的消息发布至/jointstates 话题。
- 使用 robot-state-publisher （本书281页的“对机器人建模：使用 URDF”一节有讲）将/joint_states 话题的消息转换为/tf 话题对应的消息并发布。
Figure17-1 坐标变换信息缺失下，小龟机器人的可视化图形
检查一下 /joint_states 话题下发布出的消息：
左右两个后轮的连接点位置信息输出正常，可是并没有前轮和脚轮的，怎么回事？检查 tortoisebot.urdf，注意下面这一行：
这是differential drive 插件的部分配置代码，作用是让插件向/joint_states 话题发布两个后轮（因为该插件控制的正是两个后轮）连接点的位置消息。原来如此，是插件自动输出了后轮的位置信息。那如果我们向让前轮和脚轮也一块儿发布，该怎么办呢？这就需要用到Gazebo 的另一个插件：joint state publisher。只需在 tortoisebot.urdf 中添加如 Example 17-1 所示的 URDF 代码即可。
Example17-1 加载 joint state publisher 插件，对外发布小龟机器人前轮和脚轮的位置信息
重新启动 tortoisebot.launch，使用 rostopic 监听 /joint_state：
现在就能看到所有连接点的位置数据了。接下来需要启动 robot-state-publisher。在 tortoisebot.urdf 中添加如下代码：
再次重新启动 tortoisebot.urdf，打开 rviz，效果应如图 17-2 所示。rviz 已经可以从 /tf 下获取所需的坐标变换数据，因此机器人的各个组件都能处在正确的位置上，整体看起来正常多了。
Figure17-2 正确的坐标变换信息下，小龟机器人的可视化图形
如果你想看看 robot-state-publisher 发布的消息，可以用 rostopic echo /tf。不过在这里我们介绍一种更好的做法，让 rviz 直接把数据显示出来：在rivz 的Display栏中，单击“Add”，选择 “TF”，然后再单击“OK”。现在，各个连接点对应的坐标系就会显现出来。为了能看的更清楚，可以把机器人设为半透明：在Display 栏中选择“RobotModel”，设置 Alpha 为0.5。最后的效果如图17-3所示，坐标轴分别用红，绿，蓝三色表示，坐标系的名称也标在了相应的位置上。
好，现在坐标变换完全正常，是时候给我们的机器人添加传感器了。
Figure 17-3 带有可视化坐标变换信息的小龟机器人
添加激光雷达
激光范围探测器，或称激光雷达，是移动机器人上最常见的传感器之一。它可以提供相当准确的周边环境视图。尽管它本身返回的只是某一高度上的环境“切片”（对于单线激光雷达而言确实如此），但是对于在室内环境工作的机器人而言，这些信息已经足够丰富了。进一步说，对于工作环境中有许多连续垂直结构（比如墙，门等）的机器人，激光雷达都能提供极大的帮助。在这一节中，我们会给小龟机器人添加一个激光雷达。这个激光雷达在外形上与 Hokuyo公司生产的激光雷达很像，后者是目前在机器人上运用最广泛的激光雷达之一（关于激光雷达的更多细节，请参见本书85页“激光雷达”一节）。
如果是给真实的机器人添加传感器，我们就要去完成诸如购买-安装-供电-取数据这样的工作，但是对于仿真而言，只要修改一下 URDF 就可以了。Example17-2 展示了一段 URDF 代码，其中添加了一个用来代表激光雷达的方块形连接段，并使用连接点将雷达连在了底盘的顶部中央。需要注意的是，由于我们需要用 Gazebo 对机器人进行整体仿真，因此还必须提供雷达的质量和转动惯量数据。
渐渐
Example17-2 向机器人添加代表激光雷达的连接段和相应的连接点
现在打开Gazebo 或 rviz 就可以看到我们新添加的激光雷达了。不过其实到目前为止，我们只是增加了一个方块图形，还没有告诉 Gazebo 这是一个激光雷达。为了让激光雷达名副其实，需要使用<sensor>标签来定义一个传感器。具体代码见 Example17-3。
Example17-3 定义一个激光雷达
将上面代码中的要点归纳如下：
- 首先，我们创建了一个类型为 gpu_ray 类型的传感器（名字中的“gpu”意味着我们会使用计算机的 gpu 来完成它的仿真工作，这样比使用 CPU 效率更高），并将其与之前定义的 hokuyo-link 关联起来。
- 其次，我们参照真实的Hokuyo激光雷达对其进行配置，包括：数据发布速率设为40Hz，在180˚范围内每周采样720次，探测距离最短为0.1m，最远为30m。
- 最后，我们加载 GPU laser 这个Gazebo 插件，令其对外发布激光雷达的扫描数据。数据类型为 sensor_msgs/LaserScan，话题为 scan。插件的详细文件见 gazebo-plugins documentation。
让我们来看看效果，把 Exameple17-3 中的代码插入tortoisebot.urdf，然后重新启动仿真。由于我们设置了激光雷达的探测范围，添加一些虚拟的障碍物有助于观察激光雷达的探测结果。使用 Gazebo 的图形界面拖一个圆柱体放在机器人前面即可，效果如图 17-4 所示：
再打开 rviz，在 Display 栏中选择 LaserScan，设置其 topic 为 /scan。这样就能在 rviz 中看到扫描数据了，如图 17-5 所示：
你可以在 Gazebo 中移动作为障碍物的圆柱体，或者增加新的障碍物，然后同步观察 rivz 中的扫描数据，看看变化如何。当然也可以通过 teleop-twist-keyboard 使用键盘控制机器人进行运动（本书289页“使用Gazebo进行仿真”一节中有涉及）。
Figure17-4 带有障碍物的仿真
Figure17-5 对激光雷达数据的可视化
到现在为止，小龟机器人在仿真中表现良好，坐标变换和激光雷达工作一切正常。下一步就可以增加自动导航功能了。
配置导航程序栈
本节中，我们将赋予小龟机器人在给定地图中自动导航行进的能力（本节不涉及建立地图的内容）。给一个机器人增加导航功能，需要启动三个节点：
- map_server，为机器人提供一个静态地图，用于定位和路径规划。
- amcl，使用静态地图对机器人进行定位
- move_base，进行机器人的全局路径规划和局部运动控制
ROS 导航程序栈的原理和技术细节见本书第10章。本节中我们只会讲解如何将其用于一个新的机器人，
首先需要启动的是 map_server，map_server 需要一个静态地图才能运行，我们可以复用第9章中建的地图，这个地图是某个移动机器人在一个相对较复杂的办公室中建立的（见图 17-6）。
地图存储在之前创建的 mapping 包中。要让 map_server 对外提供这张地图，请在 torroisebot.launch 的<launch>标签内添加以下代码：
我们当然也要把机器人放在一个与地图相匹配的三维仿真环境中。好在 gazebo_ros 包中正好有一个这样的环境，并且还有相应的 launch 文件。只需在 tortoisebot.urdf 中将包含 empty-world 的一行替换为下面的代码即可：
Figure17-6 用于导航的办公室地图
使用空环境进行仿真时，我们没有指定机器人的初始位置，因此使用 spawn_model 实例化时，机器人被默认放在原点。然而对于现在这个办公室环境而言，使用开阔场地作为初始位置有利于对机器人进行定位。在这里，我们选择相对环境原点 x 方向偏移8m，y 方向偏移 -8m 的位置。具体来说，只需要把 tortoisebot.launch 中调用 spawn-model 的部分替换为下面的代码即可，代码中指定了 x,y 方向的偏移。当然，你也可以用同样的办法指定相对 z 轴的偏移和机器人的初始朝向。
重新启动 tortoisebot.urdf，检查机器人是否已经被放置在了正确的位置。如果一切无误的话，改变你在 Gazebo 中的视角，应该能看到如图17-7 所示的图像。
Figure17-7 Gazebo 中处于办公室环境下的小龟机器人
再检查一下 map_serve 的工作状态，还是使用 rviz，在 Display 栏中单击“Click”，选择“Map”，然后单击“OK”；接着在 Display->Map 中设置话题为 map；同时在Display-》Global Options 中将 fixed frames 改为map。这样应该就能看到如图 17-8 所示的地图了。
Figure 17-8 rivz 中显示的静态地图
下一个启动的是 amcl。amcl 可配置性很高，而且为了提高性能，需要进行大量调整。不过对于我们而言，直接使用针对差分驱动机器人的参考配置即可。将下面的代码添加到 tortoisebot.urdf 中：
最后启动 move_base。正如第10章所述，move-base 非常复杂，能够配置的地方很多。不过好在默认配置已经能满足我们的大部分要求了，只需要做一点小小的修改。首先是设置一些在全局和局部costsmap 中都会用到的参数。创建一个名为 costmap-common-params.yaml 的文件，并插入如Example17-4 所示的代码：
Example 17-4 costsmap_common-params.yaml
首先定义的参数是机器人的外形，我们将其定义为一个包络了底盘和脚轮的矩形（可以通过添加更多的点，使之更加精确）。然后是将激光雷达设置为 observation source，这样 scan 话题上的扫描数据就会被用于更新 costsmap，包括增加新的障碍（marking）和宣告新的自由空间（clearing）。
设置好通用的参数后，我们还要分别配置全局和局部costsmap。对于全局costsmap，创建一个名为 global-costsmap-params.yaml 的文件，并插入如 Example17-5 所示的代码：
Example 17-5 global-costsmap-params.yaml
配置中，我们让全局 costsmap 使用map_server 提供的静态地图，基于 map 做路径规划。还让其在规划时考虑 base-link 所代表的机器人的实际大小。
局部costsmap只需要在上面配置的基础上稍稍改一下就可以了，创建一个名为 local-costsmap-params.yaml 的文件，并插入如 Example17-6 所示的代码：
Example 17-6 local-costsmap-params.yaml
与全局 costsmap 使用一张很大的静态地图不同，我们让局部costsmap使用的是一个移动窗口：机器人始终保持在窗口中央，窗口以外的障碍物信息被忽视。我们还让局部costsmap 基于odom 进行路径规划。这是因为相对于使用map 而言，基于odom 规划出的动作可能会出现漂移，但总体趋于平滑，而不像基于map 做规划可能会出现离散的航迹点。上述两个差异使得局部costsmap 更加适合于进行局部避障。事实上，局部避障比全局避障对机器人来说重要得多。因为前者是当下就在机器人附近发生的事，而后者则甚至可能已经不存在了（比如全局地图过时了）。
我们还需要配置一下 base local planner，是它实际完成了路径规划和控制指令计算的工作。创建一个名为 base-local-planner-params.yaml 的文件，并插入如 Exalple17-7 所示的代码：
Example 17-7 base-local-planner-params.yaml
我们只设置了一个参数：告诉 planner 小龟机器人不具备全向移动能力（因为小龟机器人的底盘采用差分驱动，关于移动机器人分类的内容请参考本书77页“执行结构：移动平台”一节）。
所有参数配置妥当，是时候修改一下launch 文件来运行 move_base 了。将 Example 17-8 中的代码添加到tortoisebot.urdf 中：
Example17-8 用于启动move_base 的 XML 代码
上述代码启动了 move_base，并使用我们刚才创建的 YAML 文件对其进行了配置。注意我们将costsmap_common-params.yaml加载了两次，一次是在global-costsmap 的名字空间，一次是在 local-map 的名字空间，这样就可以免于把同样的配置重复写两次。
这就是我们需要进行的所有配置工作。现在开始让机器人动起来吧！
使用 rviz 定位和控制导航中的机器人
所有配置文件集成好后，启动 tortoisebot.urdf，就可以在一个办公室环境中开始对小龟机器人进行仿真了，并且导航程序栈也已经准备好。如本书第10章所述，我们需要在地图上给 amcl 一个大概的初始位姿，查看 amcl 点过滤器的动态扫描过程有助于我们完成这项工作。要开启这一功能，打开 rviz 后，在 Display 栏中单击“Add”，选择“PoseArray”，再单击“OK”，并将话题设为 /particlecloud 即可。
现在同时看着 Gazebo和 rviz，直至Gazebo 中的环境和 rviz 中的地图大致匹配上。此时在 rviz 中单击 “2D pose Estimate”按钮，然后在地图中拖拽出机器人的初始位置和朝向，如图17-9 所示：
Figure17-9 在 rviz 中设置机器人的初始位姿
指定初始位姿后，在 rviz中，机器人的位姿会跳变为与你指定的位姿接近的数值。仔细查看，还会发现机器人周边环绕着许多箭头，代表 amcl 离散的位姿估计值，如图17-10所示。可以通过比较激光雷达的扫描数据和地图的差异来评判你给定初始位置的正确性。如果扫描数据与地图很不匹配，就需要重新设置初始位姿。一般来说，你需要尽可能提供最精确的初始位姿估计，不过，不够精确也没关系，因为 amcl 使用的是一个相当鲁棒的概率定位算法。与此同时，Gazebo 中还啥也没发生，因为我们还在告诉机器人它的位置，尚未指示它前往某个地方。
Figure 17-10 设置初始位姿后，rivz 中机器人的新位姿
完成定位后就可以开始让机器人开始动起来了。在 rviz 中，单击“2D Nav Goal”按钮，然后在地图中拖拽出一个目标位姿，如图17-11 所示。这样机器人就会在Gazebo 中动起来了，rviz 中也会实时更新机器人的位姿估计和激光雷达的扫描数据，如图 17-12 所示：
Figure 17-11 在 rviz 中设置导航的目标位姿
效果拔群，太好了！接下来可以做各种各样的测试，比如在机器人到达目的地后指定一个新的目标位姿；比如在一开始就给定一个完全不对的初始位姿；再比如指定一个不可达的目标位姿等等。
Figure 17-12 机器人向目标点导航中，与此同时，红色点云逐渐收敛为置信度更高的合一位姿估计值
小结
本章中，我们将一个能用的移动机器人仿真模型变成了一个可以自动导航的机器人。整个过程中没有写任何代码，只是提供了一些配置信息（使用 XML 和YAML）。这就是 ROS 的威力：诸如 robot-state-publisher，amcl 和 move-base 这样标准而灵活的 ROS 工具，可以通过配置和组合，应用在各种各样的机器人上，甚至是我们自己做的机器人。
当然，如果对我们的系统进行更多的测试，你或许会发现，小龟机器人的导航并不完美：有一定几率不能穿过门廊，有时会迷路（即定位失败），偶尔还会突然卡住不动。因此，下一步应当深入研究导航程序栈的文档，并对我们的机器人进行更仔细的配置。我们在这一章所用到的每个节点都提供了丰富的配置选项，从 amcl 运算加速限制中的噪声模型，到 move_base 进行路径规划的维度，选项众多。默认的和样例中的配置已经可以让导航系统工作起来，不过，还是要针对每个机器人进行一些参数的调整，才能保证导航性能的稳定。