ROS2与Navigation2入门教程-移动机器人导航概念

说明：

• 介绍移动机器人导航的概念，了解和使用Nav2项目所需的概念

ROS 2

• ROS2是用于Nav2的核心中间件。
• 如果您对ROS 2不熟悉，请在继续本教程之前先访问ROS 2文档。

动作服务器（Action Server）

• 就像在ROS中一样，动作服务器是控制导航等长时间运行任务的常用方式。Nav2软件堆栈更广泛地使用动作，并且在某些情况下会缺乏简单的话题接口。作为ROS 2的开发人员，了解动作服务器更为重要。一些简单的命令行界面CLI示例可以在ROS 2文档中找到。

• 动作服务器类似于规范的服务（service）服务器。客户端会请求完成某个任务，除非该任务可能需要很长时间。例如，将铲子从推土机向上移开或者让机器人向右移动10米就是这类任务的示例。

• 在这种情况下，动作服务器和客户端允许用户在另一个进程或线程中调用长时间运行的任务，并为其结果返回一个future变量。此时允许阻塞线程直到动作完成，但是，用户可能希望偶尔检查动作是否完成并继续在客户端线程中处理工作。由于是长时间运行的，动作服务器也会向其客户端提供反馈。这个反馈可以是任何事物，并且在ROS中使用具有请求和结果类型的.action文件定义。在推土机示例中，请求可能是一个角度，反馈可能是要移动的剩余角度，而结果则是以结束角度表示的成功或失败布尔值。在导航示例中，请求可能是一个位姿，反馈可能是已导航的时间和到达目标位姿的距离，而结果则是以成功或失败表示的布尔值。

• 通过向动作客户端注册回调函数，可以同步收集反馈和结果消息。也可以通过从共享的future对象异步请求信息来收集反馈和结果消息。两者都需要旋转（spinning）客户端节点来处理回调函数组。

• 在Nav2软件堆栈中使用动作服务器通过NavigateToPose动作消息与最高级别的行为树BT导航仪（Navigator）进行通信。它们还用于行为树BT导航仪与后续较小的动作服务器通信，以计算路径规划、控制工作和恢复。每个动作服务器都有自己独特的nav2_msgs格式的.action类型，用于与服务器进行交互。

生命周期节点和绑定（bond）

• 生命周期（或更准确地说是托管）节点是ROS 2独有的。更多相关信息可以在这里找到。它们是包含用于启动和关停（teardown） ROS 2服务器的状态机过渡状态的节点。这有助于确定ROS系统在启动和关闭时的行为。它还有助于用户以合理的方式构建他们的程序以用于商业用途和调试。

• 当一个节点启动时，它处于未配置（unconfigured）状态，只处理该节点的构造函数，不应包含任何ROS网络设置或参数读取。通过启动系统或提供的生命周期管理器，需要通过配置将这些节点转换过渡到非活动或非激活（inactive）状态。之后，可以通过过渡到激活阶段来激活节点。

• 激活状态允许节点处理信息并完全设置为运行。配置阶段触发的on_configure()方法会设置所有参数、ROS网络接口以及安全系统的所有动态内存分配。激活阶段触发的on_activate()方法会激活ROS网络接口并设置程序中的任何状态以开始处理信息。

• 为关闭这些节点，需要过渡到停用（deactivating）、清理、关闭并以最终状态结束。ROS网络接口分别在这些阶段被停用和停止处理、释放内存、干净地退出。

• 整个Nav2项目中广泛地使用生命周期节点框架，所有服务器都使用它。如果可能，所有ROS系统最好都使用生命周期节点。

• Nav2中使用生命周期节点（LifecycleNodes）的封装器nav2_util LifecycleNode。该封装器为典型的应用程序封装了LifecycleNodes的大部分复杂性。该封装器还包括生命周期管理器的绑定（bond）连接，以确保在服务器过渡到激活状态后，它也保持激活状态。如果服务器崩溃，它会让生命周期管理器知道并关闭系统以防止发生严重故障。有关详细信息，请参阅“从Eloquent到Foxy”。

行为树（Behavior Trees）

• 行为树（BT）在复杂机器人任务中变得越来越普遍。它们是待完成任务的树形结构。行为树为定义多步或多状态应用程序创建了一个更具可扩展性和人类可理解性的框架。行为树与有限状态机（FSM） 相对，后者可能有几十个状态和数百个状态过渡。一个例子是踢足球机器人。将足球比赛的逻辑嵌入到FSM中将非常具有挑战性，并且容易出错，因为有许多可能的状态和规则。此外，诸如从左侧、右侧或中间射门之类的建模选择尤其不清晰。使用行为树则可以为许多行为创建和重用基本原语，如“踢（kick）”、“走（walk）”、“找球（go to ball）”等。更多相关信息可以在这本书中找到。强烈建议阅读该书的第1-3章以更好地理解术语和工作流程。这应该只会花费大约30分钟时间。

• Nav2项目使用BehaviorTree CPP V3作为行为树库。在BT Navigator中创建了可以构建为行为树的节点插件。节点插件被加载到BT 中，而且在解析该行为树的XML文件时，注册的名称会被关联起来。至此，就可以通过该行为树进行导航。

• 使用BehaviorTree CPP V3库的一个原因是它能够加载子树。这意味着可以将Nav2行为树加载到另一个更高层次的行为树中，以使用此项目作为节点插件。一个例子就是在足球比赛中，使用Nav2行为树作为“找球”节点，将足球检测（detection）作为更大任务的一个部分。此外，为BT提供了一个NavigateToPoseAction插件，这样可以从客户端应用程序通过通常的动作接口调用Nav2软件堆栈。

导航服务器（Navigation Servers）

• 规划器（planner）和控制器（controller）是导航任务的核心。恢复器（recoveries）用于使机器人摆脱不良状况或者尝试处理各种形式的问题以使系统具有容错能力。在本节中，将分析有关它们的一般概念及其在Nav2项目中的使用。

规划器、控制器和恢复器服务器

• Nav2项目中的三个动作服务器是规划器、控制器和恢复器服务器。这些动作服务器用于托管一个地图算法插件以完成各种任务。它们还托管由该算法插件使用的环境表达，以计算其输出。

• 规划器和控制器服务器会在运行时使用名称（别名）和要使用的算法类型进行配置。这些类型是已注册的插件库名称，而这些名称则是任务的别名。一个示例就是使用名称FollowPath的DWB控制器，因为该控制器会跟随参考路径。在这个示例中，DWB的所有参数都会被放置在该命名空间中，例如FollowPath.。

• 这两个服务器会公开与其任务对应的动作接口。行为树在勾选相应的BT节点时，会调用动作服务器来处理其任务。该服务器内的动作服务器回调函数会通过映射到特定算法的名称（如FollowPath）调用所选算法。这允许用户将行为树中使用的算法抽象为算法类。例如，用户可以使用N个插件控制器来跟踪路径、与充电器对接、避开动态障碍物，或者与某个工具进行交互。在同一服务器中拥有所有这些插件允许用户使用单个环境表达对象，因为该对象的复制成本很高。

• 对于恢复器服务器，每个恢复器也会包含它们自己的名称，但是，每个插件也会公开它自己的具体动作服务器。这样做是因为可以创建的各种恢复动作无法共享单个简单的接口。恢复器服务器还包含一个订阅本地成本地图的成本地图订阅者节点，该节点会从控制器服务器接收实时更新以计算其任务。这样做是为了避免为本地成本地图创建多个实例，因为复制本地成本地图在计算上是昂贵的。

• 或者，既然BT节点是调用某个动作的简单插件，因此可以创建新的BT节点来调用具有其他动作类型的其他动作服务器。如果可能，建议始终使用提供的服务器。如果由于插件或动作接口的原因需要一个新的服务器，则可以通过该框架来维持。与提供的服务器类似，新服务器也应该使用新的类型和插件接口。需要创建一个新的BT节点插件来调用新的动作服务器——但是，通过广泛使用服务器和插件，不需要在Nav2存储库本身中进行分叉（forking）或修改。

• 如果您发现需要连接至插件库定义或动作类型的新接口，则请提交工单，并查看我们是否可以在相同的接口中纠正该问题。

规划器

• 规划器的任务是计算完成某个目标函数（objective function）的路径。路径也可以称为路线（route），具体取决于所选的命名法和算法。两个典型的例子就是计算一个到达目标位姿的规划（例如从当前位置到达一个目标位姿）或者完全覆盖（例如覆盖所有空闲空间的规划）。规划器可以访问全局环境表达和缓存在其中的传感器数据。

• 规划器可以被编写为具有以下功能的工具：

  • 计算最短路径
  • 计算完全覆盖的路径
  • 沿着稀疏或预定义的路线计算路径

• Nav2中规划器的一般任务是计算从当前位置到达目标位姿的一个有效且可能是最佳的路径。

• 但是，有许多受支持的规划类和路线类。

控制器

• 控制器，在ROS 1中也称为局部规划器，是跟随全局计算路径或完成局部任务的方法。控制器有权访问局部环境表达，以尝试计算要跟随的基准路径的可行控制工作。许多控制器会将机器人向前投射到空间中，并在每次更新迭代时计算局部可行路径。

• 控制器可以被编写为具有以下功能的工具：

  • 跟随路径
  • 使用里程计坐标系中的检测器与充电站（桩）对接
  • 登上升降机（电梯）
  • 与某个工具的接口

• Nav2中控制器的一般任务是计算一个有效的控制工作以跟随全局规划路径。但是，有多个控制器类和局部规划器类。Nav2项目的目标就是所有控制器算法都可以作为此服务器中的插件，以用于一般研究和产业任务中

恢复器

• 恢复器是容错系统的支柱。恢复器的目标是处理系统的未知状况或故障状况并自主处理这些状况。例子包括感知系统中会导致环境表达充满假障碍物的故障。这样就会触发清除成本地图恢复以允许机器人移动。

• 另一个例子就是机器人由于动态障碍物或控制不佳而卡住。在允许的情况下，倒退或原地旋转会允许机器人从卡住的位置移动到可以成功进行导航的自由空间中。

• 最后，在完全故障的情况下，为寻求帮助，可以实施恢复以引起操作人员的注意。这可以通过电子邮件、短信、Slack、Matrix等来完成。

航点跟随（Waypoint Following）

• 航点跟随是导航系统的基本功能之一。它会告知系统如何使用导航程序到达多个目的地。

• 软件包nav2_waypoint_follower包含一个航点跟随程序，该程序具有用于特定任务执行器的插件接口。如果需要让机器人前往给定位姿并完成像拍照、捡起盒子或等待用户输入之类的特定任务，这会非常有用。这是一个用于展示如何在示例应用程序中使用Nav2的不错的演示应用程序。

• 但是，该软件包并不仅仅可以用于示例应用程序。关于机器人队管理器/调度器（fleet managers/dispatchers）有两种思想流派：①哑机器人+智能集中式调度器；②智能机器人+哑集中式调度器。

• 在第一种思想流派路中，软件包nav2_waypoint_follower完全足以创建产品级的机器人解决方案。由于自主系统/调度器在分配任务时会考虑机器人的位姿、电池电量、当前任务等各种因素，因此机器人上的应用程序只需要关心手头上的任务，而不用关心完成系统要求任务的其他复杂因素。在这种情况下，应该将发送至航点跟随者的请求视为1个工作单元（例如，仓库中的1次拣货、1个安全巡逻循环、1个过道等）来执行任务，然后返回给调度器以进行下一个任务或者要求充电。在这种思想流派中，航点跟随应用程序只是导航软件堆栈之上和系统自主应用程序之下的一个步骤。

• 在第二种思想流派中，软件包nav2_waypoint_follower是一个不错的示例应用程序/概念证明（proof of concept），但确实需要机器人上的航点跟踪/自主系统来承担更多任务以制定健壮的解决方案。在这种情况下，应该使用nav2_behavior_tree软件包创建自定义应用程序级别的行为树，以使用导航来完成任务。这可以包含子树，例如在任务中检查充电状态以返回停泊坞（dock），或者在更复杂的任务中处理1个以上的工作单元。很快就将会有一个nav2_bt_waypoint_follower软件包（名称可能会调整），让用户可以更轻松地创建此应用程序。在这种思想流派中，航点跟随应用程序与自主系统的联系更加紧密，或者在很多情况下，航点跟随应用程序就是自主系统本身。

• 这两种思想流派并不能简单地说谁比谁更好，谁更好很大程度上取决于机器人正在完成何种任务、处于何种类型的环境中以及有何种可用的云资源。通常，对于既定的业务案例，这种区别非常明显。

状态估计（State Estimation）

• 根据ROS社区标准，在导航项目中，要求提供2个主要的坐标变换。由map到odom的坐标变换由定位系统（定位localization, 制图mapping, 即时定位与制图SLAM）提供，而odom到base_link的坐标变换则由里程计系统提供。

• 注：无需在机器人上使用LIDAR即可使用导航系统。不需要使用基于激光雷达的防撞、定位或SLAM系统。但是，Nav2确实可以提供说明和支持使用激光雷达对这些系统进行尝试和真实实现。使用基于视觉或深度传感器的定位系统和使用其他传感器来避免碰撞可以同样成功。唯一的要求就是在选择具体实现方式时遵循以下标准。

标准

• REP 105定义了导航和更大的ROS生态系统所需的坐标系及其约定。应始终遵循这些约定，以利用社区中可用的丰富的定位、里程计和 SLAM项目资源。

• 简而言之，REP-105告知您必须至少为您的机器人构建一个包含map -> odom -> base_link -> [sensor frames]的完整TF树。TF2是 ROS 2中的时变（time-variant）坐标变换库，Nav2使用TF2来表达和获取时间同步的坐标变换。全局定位系统（GPS、SLAM、动作捕捉Motion Capture）的工作至少要提供map -> odom坐标变换。然后里程计系统的作用就是提供odom -> base_link坐标变换。与base_link相关的其余坐标变换应该都是静态的，并且应该在URDF中定义。

全局定位：定位（Localization）和SLAM

• 全局定位系统（GPS、SLAM、动作捕捉Motion Capture）的工作至少要提供map -> odom坐标变换。Nav2提供了amcl工具，这是一种基于粒子过滤器的自适应蒙特卡罗定位（Adaptive Monte-Carlo Localization，AMCL）技术，用于静态地图的定位。Nav2还提供了SLAM工具箱（SLAM Toolbox）作为SLAM默认算法，用于定位和生成静态地图。

• 这些方法也可能产生其他输出，包括位姿话题、地图或其他元数据，但它们必须提供坐标变换才能有效。可以使用机器人定位（robot localization）将多种定位方法融合在一起，下面将会对其详细讨论。

里程计（Odometry）

• 里程计系统的作用就是提供odom -> base_link坐标变换。里程计可以来自许多数据源，包括激光雷达LIDAR、雷达RADAR、车轮编码器、VIO和IMU。里程计的目标是提供一个基于机器人运动的平滑、连续的局部坐标系。全局定位系统会更新相对于全局坐标系（global frame）的坐标变换以解决里程计漂移问题。

• 机器人定位（Robot Localization）通常被用于这种融合。它会采用N个各种类型的传感器，并为TF和一个话题提供连续且平滑的里程计。一个典型的移动机器人装置可能具有来自车轮编码器、IMU的里程计以及融合在该范围内的视觉。

• 这样平滑输出就可用于精确运动的航行位置推算和在全局位置更新之间准确地更新机器人的位置。

环境表达（Environmental Representation）

• 环境表达是机器人感知环境的方式。它还充当各种算法和数据源的中心定位工具，以将它们的信息组合到单个空间中。这样，控制器、规划器和恢复器就可以使用该空间来安全有效地计算它们的任务。

成本地图（Costmaps）和图层（Layers）

• 当前的环境表达是一个成本地图。成本地图是一个规则的2D单元格网格，这些单元格中包含来自未知、空闲、占用或膨胀成本的成本。然后会搜索此成本地图以计算全局路径规划或进行采样以计算局部控制工作。

• 各种成本地图图层被实现为pluginlib插件，以将信息缓冲到成本地图中。这包括来自LIDAR、RADAR、声纳、深度传感器、图像等的信息。在将传感器数据输入到成本地图图层之前对其进行处理可能是明智的，但这取决于开发人员。

• 可以创建成本地图图层来检测和跟踪场景中的障碍物，以便使用相机或深度传感器来避免发生碰撞。此外，可以创建图层以基于某些规则或启发式算法更改底层成本地图。最后，它们可用于将实时数据缓冲到2D或3D世界中，以进行障碍物的二值化标记。

成本地图过滤器

• 想象一下，您正在注释地图文件（或任何图像文件），以便根据注释后地图中的位置执行特定动作。标记/注释的示例可以是将某些区域排除在外以避免在这些区域内进行路径规划，或者让某些像素属于标记区域中的最大速度。这种带注释的地图被称为“过滤器掩码（filter mask）”。就像覆盖在某个表面上的掩码一样，过滤器掩码的大小、位置和比例尺可以与主地图相同，也可以不同。过滤器掩码的主要目标是提供在地图上进行标记的能力，以标记具有某些附加功能或行为变化的区域。

• 成本地图过滤器是基于成本地图图层的方法，可以将过滤器掩码中注释的空间相关行为变化应用到Nav2软件堆栈中。成本地图过滤器是作为成本地图插件实现的。这些插件之所以被称为“过滤器”，是因为它们会通过过滤器掩码上标记的空间注释对成本地图进行过滤。为了制作过滤后的成本地图并改变机器人在注释区域的行为，过滤器插件会读取来自过滤器掩码的数据。该数据在过滤器空间中被线性变换为特征图。有了这个变换后的特征图以及地图/成本地图、任何传感器数据和机器人当前坐标，过滤器就可以更新底层成本地图，并根据机器人的位置改变机器人的行为。

• 例如，通过使用成本地图过滤器可以实现以下功能：

  • 机器人永远不会进入的禁区/安全区。
  • 限速区：进入这些区域的机器人最大速度将会受到限制。
  • 机器人在工业环境和仓库中移动的首选通道。

其他形式

• 存在各种其他形式的环境表达，包括：

  • 梯度图，类似于成本地图，但梯度图会表达表面梯度以检查可穿越性（traversibility）；

  • 3D成本地图，以3D形式表示空间，但这样就也需要3D规划和碰撞检测；

  • 网格（mesh）图，类似于梯度图，但具有多个角度的表面网格；

  • “矢量空间”，接收传感器信息并使用机器学习算法来检测要跟踪的单个物品（item）和位置，而不是对离散点进行缓冲区计算。

Nav2学术概述（Academic Overview）

• 有关Nav2的学术概述，请观看视频

参考：

• https://navigation.ros.org/concepts/index.html