navigation2的几个概念

navigation2中的几个概念

1.Action Server(动作服务器)

Action Server是在ROS中控制导航等长时间运行任务的常见任务。

Action服务器类似于服务通信中的Service，客户端会要求服务端完成一些任务，但此任务可能需要很长时间。在这种情况下，动作服务器和客户端允许我们在另一个进程或线程中调用长时间运行的任务，并为其结果返回一个future对象。此时允许在操作完成之前阻止，但是，用户可能希望偶尔检查动作是否完成并继续在客户端线程中处理工作。由于是长时间运行的，动作服务器也会向其客户端提供反馈。该反馈可以是任何内容，并在ROS中使用具有请求和结果类型的.action文件定义。在导航任务中，请求可能是一个位姿，反馈可能是已导航的时间和到达目标位姿的距离，而结果则是以成功或失败表示的布尔值。

通过向Action客户端注册回调函数，可以同步的收到反馈和结果。它们也可以通过异步请求从shared future对象中收集信息。两者都需要对客户端节点进行spin来处理回调组。

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

简单地说，Action Server的工作机制是首先客户端向Action Server发送一个请求，Action Server接收到请求后开始执行相应的任务，在任务执行过程中，Action Server会定期向客户端发送反馈（至于反馈的是何内容是在.action文件中定义的）告知任务的当前进度。任务完成后，Action Server向客户端发送结果并告知任务成功或失败。

2.Lifecycle Node(周期节点)

Lifecycle Node是包含状态机转变的用于加载和卸载ROS2服务器的节点。这有助于确定ROS系统启动和关闭的状态是否正常。

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

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

要关闭该节点，我们需要过渡到停用、清理、关闭，并以Finalized状态结束。网络接口在这些阶段被停用并分别停止处理、释放内存、彻底退出。

在Nav2中，我们使用nav2_util LifecycleNodes的包装器。这种包装器为典型应用包装了生命周期节点的许多复杂性。它还包括生命周期管理器的连接bond，以确保服务器转换后，它也保持活动状态。如果服务器崩溃，它会让生命周期管理器知道并向下过渡系统，以防止严重故障。

有四个预定义状态：unconfigured, inactive, active, finalized.

Unconfigured:初始节点，节点已创建但未进行任何配置。此状态下，节点无法提供服务或处理数据。

Inactive:节点已配置但未激活。节点在此状态下可以接收配置参数和执行初始化，但不执行主要任务。

Active:节点已激活并正常运行。在此状态下，节点执行主要任务，如处理数据、发布话题等。

Finalized:节点已终止。此状态表示节点的生命周期已结束。

Unconfigured -> Inactive: configure,使用on_configure()函数初始化节点，配置参数和资源；

Inactive -> Active: activate,使用on_activate()函数激活节点；

Active -> Inactive:deactive,使用on_deactivate()函数暂停节点；

Inactive -> Unconfigured:cleanup,使用on_cleanup()函数清理节点；

Inactive/Unconfigured -> Finalized:shutdown,使用on_shutdown()函数关闭节点。

还有一个可调用函数on_error():在状态转换失败时调用，用于处理错误情况。

3.Behavior Trees(行为树)

Behavior Trees是待完成任务的树形结构。

Behavior Trees在复杂的机器人任务中变得越来越普遍。它们是待完成的树形结构。行为树为定义多步或多状态应用程序创建了一个更具可扩展性和人类可理解性的框架。这与有限状态机(FSM)相反，后者可能有几十个状态和数百个状态过渡。一个例子是踢足球机器人。将足球比赛的逻辑嵌入FSM将具有挑战性，且容易出错因为有许多可能得状态和规则。此外，像从左侧、右侧或中间射门这样的建模选择尤其不清楚。使用行为树则可以为许多行为创建和重用基本源语，像"kick"“walk”“go to ball”。

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

使用此库的一个原因是它能够加载子树，这意味着可以将Nav2行为树加载到另一个更高级别的BT中，以将此项目中用作节点插件一个例子是在足球比赛中，使用Nav2行为树作为"go to ball"节点，将足球检测作为更大任务的一部分。此外，为BT提供了一个NavigationToPoseAction插件，因此可以从客户端应用程序通过通常的动作接口调用Nav2软件堆栈。

在ROS2中，行为树的使用主要是通过ROS2 Action来实现的。这是因为ROS2 Action满足行为树中节点中反应性行为的基本需求：异步的，并且它们可以被中止。Action是ROS2中的一种通信机制，适用于长时间运行的任务，可参考上方Action Server.为了在ROS2中使用行为树，首先需要对原始的行为树库即BehaviorTree.CPP进行封装，使其可以被ROS2 Action类调用。Nav2包中封装好的是nav2_behavior_tree。

行为树本身并不具体实现机器人的执行内容，它只负责将执行内容进行编排。在Nav2中，具体的执行内容是放在各个服务器Server中的。行为树上的节点与服务器进行通信，请求具体的执行内容，然后获得反馈。根据反馈结果又可以请求另外的执行内容。这些不同的执行内容之间的跳转就是由行为树控制的。

4.Navigation Servers(导航服务器)

Navigation Servers包含三个动作服务器：the planner, recovery, and controller servers. 这些服务器用于托管一个地图算法插件，以完成各种任务。它们还托管由该算法插件使用的环境表达，以计算其输出。

规划器和控制器是导航任务的核心。恢复器用于使机器人摆脱不良状态或尝试处理各种形式的问题，以使系统具有容错能力。

Planner规划器和Controller控制器将在运行时配置名称（别名）和要使用的算法类型。这些类型是已注册的插件库名称，这些名称是任务的别名。一个例子是使用名称为FollowPath的DWB控制器，因为它遵循参考路径。在这种情况下，DWB的所有参数都将放置在该命名空间中，例如FollowPath.<param>。

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

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

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

Planner

​ planner:规划器的任务是计算完成一些目标函数的路径。根据所选的命名法和算法，该路径也可以称为路线。两个典型示例是计算一个到达目标位姿的规划（例如从当前位置到达一个目标点）或者完全覆盖（例如覆盖所有空闲空间的规划）。规划器可以访问全局环境表达和缓存在其中的传感器数据。规划器可以被编写为具有以下功能的工具：

· 计算最短路径

​ · 计算完整覆盖路径

​ · 沿稀疏或预定义路线计算路径

Nav2中规划器的一般任务是计算从当前位置到达目标位姿的一个有效且可能是最佳的路径。但是，有很多受支持的规划类和路线类。

Controller

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

· 跟随路径

· 使用里程作为与充电桩对接时的参考数据

​ · 登上电梯

​ · 与某个工具的接口

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

Recovery

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

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

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

需要注意的是，行为服务器可以保留任何行为，以共享对代价地图或TF缓冲区等昂贵资源的访问，而不仅仅是恢复行为。每个都可以有自己的API。

Smoother

Smoother(平滑器)：由于与实际情况相比，规划器搜素的路径的最优标准通常会降低，因此额外的路径细化通常是有益的。为此目的引入了平滑器，通常负责减少路径粗糙和平滑突然旋转，但也是为了增加与障碍物和高成本区域的距离，因为平滑者可以获得全球环境代表。

当将不同的规划器与不同的平滑器组合在一起时，或者当需要对平滑进行特定控制时，例如仅平滑路径的特定部分，使用单独的平滑器而不是作为规划器的一部分的平滑器是有利的。

为了更平滑，Nav2中的一般任务是接收路径并返回其改进版本。但是，对于不同的输入路径，存在改进标准和获取它们的方法，从而为可以在该服务器中注册的大量平滑器创造了空间。

Robot Footprints

Robot Footprints(机器人足迹)：在代价地图中，如果机器人是非圆形的，我们将机器人的足迹设置为半径robot_radius圆或表示任意多边形的点向量。这也可以使用代价地图的~/footprint话题随时间推移进行调整，该话题将根据机器人状态的变化（例如连接的机械手的移动、拾取托盘或其他调整机器人形状的操作）随着时间的推移更新多边形。然后，规划器和控制器将自动使用该多边形。

Waypoint Following

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

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

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

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

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

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

nav2_waypoint_follower当使用robot_localization的navsat_transform节点提供全局定位时，还支持GPS航点跟踪，但也可由Fuse或任何其他来源提供。其中有一个nav2_waypoint_follower中的名为/follow_gps_waypoints的动作服务器(action server)，可以直接接收以GPS坐标表示的目标，将它们转换为全局坐标系中的笛卡尔坐标目标，并将它们作为笛卡尔航点执行。

5.State Estimation(状态估计)

在导航项目中，需要提供两个主要的坐标转换。map到odom的坐标变换由定位系统（定位，建图，SLAM）提供，odom到base_link的坐标转换由里程计提供。

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

标准

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

简而言之，REP-105至少必须为机器人建造一个包含map->odom->base_link->[sensor frames]的完整的TF树。TF2是ROS2中的时变坐标变换库，Nav2使用TF2来表达和获取时间同步的坐标变换。全球定位系统(GPS、SLAM、动作捕捉Motion Capture)的工作是至少要提供map->odom的坐标转换。然后，里程计系统的作用是提供odom->base_link的坐标转换。关于base_link的其余坐标转换应该是静态的，并应在URDF中定义。

Global Positioning: Localization and SLAM(全局定位：定位与SLAM)

全局定位系统（GPS、SLAM、动作捕捉）的工作是至少提供map->odom的坐标转换。Nav2项目提供的amcl是一种基于粒子过滤器的自适应蒙特卡洛定位技术，用于静态地图的定位。Nav2还提供用于定位和生成静态映射的SLAM工具箱作为默认的SLAM算法。

这些方法还可能产生其他输出，包括位置话题、地图或其他元数据，但它们必须提供该转换才能有效。使用机器人定位可以将多种定位方法融合在一起，下面将详细讨论。

Odometry(里程计)

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

这robot_localization通常用于这种融合。它将采用各种类型的N个传感器，并为TF和话题提供连续平滑的里程计。一个典型的移动机器人装置可能有来自车轮编码器或IMUs的里程计以及融合在这个工作区内的视觉。

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

6.Environmental Representation(环境表达)

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

Costmaps and Layers(代价地图和图层)

当前的环境表达是一个代价地图。代价地图是包含来自未知、空闲、占用或膨胀成本的单元格的规则2D单元格网格。然后该代价地图被寻找以计算全局规划或采样以计算局部控制工作。

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

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

Costmap Filters(代价地图过滤器)

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

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

​ · 机器人永远不会进入的禁区/安全区域。

​ · 限速区域。机器人进入这些区域的最大速度将受到限制。

​ · 机器人在工业环境和仓库中移动的首选车道。

其他形式

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

​ < 梯度图>，类似于代价地图，但可以表达表面梯度，以检查可穿越性

​ <3D代价地图>，以3D形式表示空间，但还需要3D规划和碰撞检测

​ <网格图>，类似于梯度图，但具有多个角度的表面网格

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