navigation2学习笔记

ros2的navigation比起ros1修改了很多内容，导航模块化的思想保留了下来，接下来我们就来看一下这里面的改动吧。

在学习Navigation2之前，需要对ROS 2的基本概念有清晰的了解，包括节点、话题、服务、动作、生命周期等。

nav2体验

安装好对应的内容，我们先来体验一下ros2中的navigation2的工作过程，这一部分与ros1几乎没有太大的区别，只需要注意自己的版本即可。我使用的是ros2 galactic的版本。

sudo apt install ros-galactic-navigation2

sudo apt install ros-galactic-nav2-bringup

sudo apt install ros-galactic-turtlebot3*

打开一个终端初始化一下车辆

export TURTLEBOT3_MODEL=waffle
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/galactic/share/turtlebot3_gazebo/models

这是让gazebo能够加载对应的模型，然后启动我们的nav2的仿真：

ros2 launch nav2_bringup tb3_simulation_launch.py

就会启动一个gazebo环境和一个rviz界面，相信之前用过nav1的对这个环境应该不陌生


它的定位也是通过amcl实现的，所以我们通过2D pose estimate来给一个初始的位姿，然后就能够成功定位上了，同时nav2也处于active状态：

接下来我们通过rviz中的nav2 goal给一个目标点即可作为导航终点。此时车辆运动起来来，调用BT nav server来控制车辆到达指定的位置，同时可以通过rviz中的可视化工具进行取消该动作。

生命周期管理

这里的节点,话题,动作,服务都是在ros1中的概念,所以我们主要看生命周期是什么.在上图中有个符号来表示lifeclcle manager的,就是节点的生命周期.通俗来说,就是一个节点专门是记录当前的节点所处的状态,节点也是根据这个状态机来运行.
在ROS 2中，节点的生命周期是指节点从创建到销毁的整个过程。生命周期管理允许节点在运行时根据需要动态地改变其状态。例如，节点可以在某些条件下暂停执行，或者在资源不足时释放资源。
这是ros官方的一个说明生命周期的状态图，一个节点的完整生命周期的定义就是类似于这张图中的内容。

其作用在于:在允许任何组件开始执行其行为之前，确保所有组件都已正确实例化。它还将允许节点重新启动或在线更换。

那么ros2中是怎么定义这些一系列的状态，并且在状态之间进行转换的呢？我们通过python ros2 来说明这个过程。

import rclpy
from rclpy.lifecycle import LifecycleNode, LifecycleState, TransitionCallbackReturn
from std_msgs.msg import String

class LifecycleTalker(LifecycleNode):
    def __init__(self):
        super().__init__('lifecycle_talker')
        self.publisher = None
        self.timer = None

    def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn:
        self.get_logger().info('Configuring node...')
        self.publisher = self.create_publisher(String, 'lifecycle_chatter', 10)
        return TransitionCallbackReturn.SUCCESS

    def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn:
        self.get_logger().info('Activating node...')
        self.timer = self.create_timer(1.0, self.timer_callback)
        return TransitionCallbackReturn.SUCCESS

    def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn:
        self.get_logger().info('Deactivating node...')
        self.timer.cancel()
        return TransitionCallbackReturn.SUCCESS

    def on_cleanup(self, state: LifecycleState) -> TransitionCallbackReturn:
        self.get_logger().info('Cleaning up node...')
        self.destroy_publisher(self.publisher)
        return TransitionCallbackReturn.SUCCESS

    def on_shutdown(self, state: LifecycleState) -> TransitionCallbackReturn:
        self.get_logger().info('Shutting down node...')
        return TransitionCallbackReturn.SUCCESS

    def timer_callback(self):
        msg = String()
        msg.data = 'Hello, world!'
        self.publisher.publish(msg)
        self.get_logger().info('Published: "%s"' % msg.data)

def main(args=None):
    rclpy.init(args=args)
    node = LifecycleTalker()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()

定义一个生命周期节点，需要继承原有的LifecycleNode类，然后定义了一些状态，包括：

• 未配置（Unconfigured）
  这是节点的初始状态。节点处于此状态时，尚未初始化任何资源。
• 未激活（Inactive）
  节点处于此状态时，已经完成了基本配置，但尚未开始执行其主要功能。
• 激活（Active）
  节点处于此状态时，已经开始执行其主要功能。
  在此状态下，节点可以发布消息、调用服务或处理动作。
• 停用（Deactivated）
  节点处于此状态时，已经停止执行其主要功能，但仍然保持配置状态。
  在此状态下，节点可以释放资源，但不会完全销毁。
• 未配置（Unconfigured）
  节点可以再次进入未配置状态，重新进行配置。
• 最终（Finalized）
  节点处于此状态时，已经完全销毁，无法再恢复到其他状态。
  在官方的教程中，对生命周期节点和绑定的描述如下：
  生命周期节点是ROS 2独有的。它们是包含状态机转换的用于加载和卸载ROS 2服务器的节点。这有助于确定ROS系统启动和关闭的状态是否正常。
  当一个节点启动时，它处于未配置状态，只处理节点的构造函数，该构造函数不应包含任何 ROS 网络设置或参数读取。通过启动系统或提供的生命周期管理器，需要通过配置将节点转换为非活动状态。之后，可以通过激活阶段的转换来激活节点。
  这种状态将允许节点处理信息，并完全设置为运行状态。在配置阶段，触发 on_configure() 方法，将设置所有参数、ROS网络接口，以及安全系统，所有动态内存的分配。在激活阶段、触发 on_activate() 方法的将激活ROS网络接口，并设置程序中的任何状态以开始处理信息。
  要关闭（该节点）即过渡到停用需要清理、关闭，并以最终状态结束。网络接口分别在这些阶段被停用并停止处理、释放内存、干净地退出。
  生命周期节点框架在整个项目中被广泛使用，所有服务器都使用它。如果可能的话，所有ROS系统最好使用生命周期节点。
  在Nav2中，我们使用 nav2_util LifecycleNode 的包装器。这种包装包装了典型应用的生命周期节点的许多复杂性。它还包括生命周期管理器的连接 bond ，以确保服务器转换后，它也保持活动状态。如果服务器崩溃，它会让生命周期管理器知道并向下过渡系统，以防止严重故障。

BT navigator server

基于可配置的行为树(BT tree)的导航框架中，是一个用来管理整个server的模型执行和任务规划器，作为决策的一种，可以类比于有限状态机，但是它又比状态机更加好用。它主要是使用Behavior Tree的cpp库，通过加载xml的配置文件来将行为配置到具体的执行服务器中。也就是规划（Planner）、控制（controller）、恢复（recovery server）。按照官方的教程来说，就是行为树 (BT) 在复杂的机器人任务中变得越来越普遍。

它们是待完成任务的树形结构。行为树为定义多步或多状态应用程序创建了一个更具可扩展性和人类可理解性的框架。这与有限状态机 (FSM) 相反，后者可能有几十个状态和数百个状态过渡。使用行为树则可以为许多行为创建和重用基本原语，像 “kick” “walk” “go to ball” 。

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

使用此库的一个原因是它能够加载子树。这意味着可以将Nav2行为树加载到另一个更高级别的BT中，以将此项目用作节点插件。此外，为BT提供了一个 NavigateToPoseAction 插件，因此可以从客户端应用程序通过通常的动作接口调用Nav2软件堆栈。

动作服务器

这个其实在ros1中就已经初步学习过了，从本质上来看是一个服务，但是除了调用服务之外，还能管理服务的执行，同时也能看到执行的feedback。

也就是说，动作服务器和普通服务通讯类似，只不过它是可以被取消的，而且是允许在通讯过程中实时进行进度反馈(feedback)的。

动作服务器就像在ROS中一样是控制导航等长时间运行任务的常见方式。包含了请求，反馈和结果三部分内容。Nav2软件堆栈更广泛地使用动作(通信机制)，并且在某些情况下会缺乏简单的话题接口。
这个Action服务器类似于服务通信中的Service。客户端会要求服务端完成一些任务，但此任务可能需要很长时间。
在这种情况下，动作服务器和客户端允许我们在另一个进程或线程中调用长时间运行的任务，并为其结果返回一个future对象。此时允许阻塞线程直到动作完成，但是，用户可能希望偶尔检查动作是否完成并继续在客户端线程中处理工作。由于是长时间运行的，动作服务器也会向其客户端提供反馈。该反馈可以是任何东西，并且在ROS中使用具有请求和结果类型的.action文件定义。在导航示例中，请求可能是一个位姿，反馈可能是已导航的时间和到达目标位姿的距离，而结果则是以成功或失败表示的布尔值。
通过向Action客户端注册回调函数，可以同步的收到反馈和结果。它们也可以通过无意识地从shared future对象中的require信息来获取。两者都需要对客户端节点进行spin来处理回调组。
在Nav2软件堆栈中使用动作服务器通过 NavigateToPose 动作消息与最高级别的行为树BT导航仪（Navigator）进行通信。它们还用于行为树BT导航仪与后续较小的动作服务器通信，以计算路径规划、控制工作和恢复。每个动作服务器都有自己独特的 nav2_msgs 格式的 .action 类型，用于与服务器进行交互。

之前我们说过BT tree了，这里的动作服务器与BT也是有很大的关联的，动作服务器通过NavigateToPose动作消息与最高级别的行为树 (BT) 导航器进行通信。它们还用于 BT 导航器与后续较小的动作服务器进行通信，以计算计划、控制工作量和恢复。每个动作服务器都有自己独特的.action类型来nav2_msgs与服务器进行交互。

导航服务器

到这里就是nav2中组织各模块的方式了，包括规划控制，恢复，航点跟随等等。导航服务器就是各个功能模块的动作服务器的具体实现形式。

Planners

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

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

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

Controllers

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

• 跟随路径

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

恢复器

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

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

最后，在完全故障的情况下，可以实施恢复以引起操作员的注意以寻求帮助。

航点跟随

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

nav2_waypoint_follower 软件包含一个航路点跟踪程序，该程序具有特定任务执行程序的插件接口。如果需要让机器人前往给定位姿并完成像拍照、捡起盒子或等待用户输入之类的特定任务，这会非常有用。

到这里我们就介绍完了这张图的大部分内容了：

当然还缺少两个重要的模块：状态估计和环境表达，这个与ros1的基本没有区别，这里只进行最基本的说明。

状态估计

nav中的tf主要提供两个重要的变换，一个是map->odom,一般由slam给出。
另一个则是odom到base_link，由odom里程计提供。
这里是通过Tf坐标转换进行的，tf全称是transforms，定义了不同坐标系之间的关系，包括平移旋转和相对运动。nav中的tf提供者如下：
map->odom(AMCL)
odom->base_link(里程计)
base_link-> base_laser（传感器基架、urdf）
通常我们是使用tf2，也就是第二代的tf库，包含的模块有：
tf2_bullet
tf2_eigen
tf2_geometry_msgs
tf2_kdl
tf2_sensor_msgs
通过上述模块，我们可以轻松对tf坐标信息进行广播，发布。

这里提到了里程计，我们不需要把它想得太复杂，他就是一个根据机器人运动提供局部精确的机器人姿态和速度估计的模块，通常可以使用轮式里程计，IMU，激光雷达或者VIO等。在ros中则是使用robot_localization对多传感器系统进行里程计的模拟。常见的机器人设置至少包括车轮编码器和 IMU 作为其里程计传感器源。为此robot_localization能够通过使用状态估计节点融合传感器提供的里程计信息。这些节点使用扩展卡尔曼滤波器 ( ekf_node) 或无迹卡尔曼滤波器 ( ukf_node) 来实现这种融合。此外，该包还实现了navsat_transform_node在使用 GPS 时将地理坐标转换为机器人的世界坐标系。

环境表示

2d的环境地图与ros1是一样的分层结构，通过传感器数据，将地图以grid map的形式提供了环境表示。占用网格中的单元格存储的是0-254之间的带价值，这些代价值表示的是穿越这些区域的成本。
0是空闲状态，254是占据状态。代价地图一般是由多个层组成的。每层都具有一定的功能，最终都是为了计算总成本。nav过程中是通过nav2_costmap_2d包实现的，而该包由以下层组成，但基于插件，允许自定义和使用新层：静态层、膨胀层、范围层、障碍物层和体素层。

参考

ros2介绍视频
ros2中文教程
官方文档
生命周期说明
实例：动作服务器