10天速通Navigation2(一) 框架总览和概念解释

前言

• 本教程将贯穿整个navigation2，从原理到代码实现进行说明。
• 本教程默认读者拥有ROS2和C++的基础。
• 作者是ROS1和Nav1的忠实爱好者，故可能对一些较为常识或简单的概念不做详细介绍，大家自己搜咯，请多多包涵~

Navigation1回顾

• 基于move_base
  • 不可配置的状态机
  • 单进程导航系统
  • 只允许单一全局和局部规划器
  • 只适配于差速和全向轮
• DWA或者TEB局部路径器
• 基于Dijstra的全局路径器
• Costmap2D2维代价栅格地图

Navigation2一览

• 基于可配置的行为树(BT tree)的导航框架
• 独立的导航模块化直接可替换件
• 多元的局部路径和路径规划器
• 恢复行为Recovery Server
• 支持2D/3D位姿评估

导航行为树简介

• 行为树(BT tree):
  • 一种基于树状的模型执行模型和任务规划器
  • 可以把他理解成有限状态机FSM的进阶版
• 使用Behavior Tree.CPP库
  • 插件节点(Plugin nodes):control,action,decorator,condition
  • 自定义节点(Custom nodes)用于修改或扩展导航行为
• BT Navigator
  • 通过加载自定义行为树配置文件XML
  • BT节点将会把自定义行为分布配置到具体的执行服务器，例如(Planner,Controller,Recovery Server)

快速安装

• 官方提供了快速安装包的方式

sudo apt install ros-<ros2-distro>-navigation2
sudo apt install ros-<ros2-distro>-nav2-bringup

• 这里推荐使用humble版本进行配置(因为它的版本bug最少)

ROS2前置知识回顾

动作服务器Action Server

• 和ROS1一样通常动作服务器由目标goal，反馈feedback和结果result组成，下述是.action文件的组成

# Request 
int32 order
---
# Result
int32[] sequence
---
# Feedback
int32[] partial_sequence

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

• 动作服务器Action Server在Nav2的使用: 动作服务器通过NavigateToPose动作消息与最高级别的行为树 (BT) 导航器进行通信。它们还用于 BT 导航器与后续较小的动作服务器进行通信，以计算计划、控制工作量和恢复。每个动作服务器都有自己独特的.action类型来nav2_msgs与服务器进行交互。

• 这里我们不对如何编写直接进行说明，感兴趣可以移步至官方教程->ROS2动作服务器

生命周期节点和绑定

• 生命周期节点(Lifecycle nodes)是ROS2独有的新机制。

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

• 做重要的是，被管理节点(a managed node)呈现已知的接口，根据已知的生命周期状态机执行，它不是一个黑箱子!!!
  

• 上述图片展示了一个节点的生存周期的全部状态，通常一个节点包含着以下4个基础的基础状态(primary state)

  • Unconfigured:未配置状态，这是节点在实例化后立即处于的生命周期状态。这也是节点在发生错误后可以重新调整到的状态。
  • Inactive：未活跃状态，允许节点（重新）配置（更改配置参数、添加和删除主题发布/订阅等），而不改变其运行时的行为。在这个状态下，节点不进行任何处理(包括订阅，回调，响应等等)
  • Active：活跃状态，节点执行任何处理、响应服务请求、读取和处理数据、产生输出等。
  • Finalized：是节点在被销毁之前立即结束的状态。此状态用于支持调试和自检。

• 此外节点还提供了 6 个过渡状态和7 种转变，分别可以通过对应的函数进行状态转换。以下是6种过渡状态

  • Configuring：配置ing过渡状态，将调用节点的onConfigure回调以允许节点加载其配置并进行任何所需的设置。节点的配置通常涉及节点生命周期内必须执行一次的任务，就在这里完成~
  • CleaningUp：清理过渡态，将调用节点的回调onCleanup。此方法应清除所有状态并将节点返回到与首次创建时功能相同的状态。如果无法成功完成清理，它将过渡到ErrorProcessing。
  • ShuttingDown关闭过渡态，将执行回调onShutdown。此方法应在销毁前进行必要的清理。
  • Activating：激活过渡态，将执行回调onActivate。此方法应完成开始执行的任何最终准备工作。
  • Deactivating：去激活过渡态，将执行回调onDeactivate。此方法应执行任何清理操作以开始执行，并应撤消更改onActivate。
  • ErrorProcessing：错误处理过渡态。如果错误处理成功完成，则节点可以返回到Unconfigured，如果无法完全清除，则必须失败，并且节点将过渡到Finalized以准备销毁。
    *生命周期节点(Lifecycle nodes)在Nav2的使用:在 Nav2 中，我们使用 LifecycleNodes 的包装器。它包括一个生命周期管理器连接，以确保服务器启动后仍保持活动状态。如果服务器崩溃，它会通知生命周期管理器并关闭系统以防止发生严重故障。

导航服务器

• 通常导航服务器Navigation Servers由下面四个动作服务器组成：

  • Planners规划器
  • Controllers控制器
  • Behaviors行为器
  • Smoothers平滑器

• 他们在nav2被高度抽象化和模块化独立出来，其中Planners、Smoothers和Controllers将在运行时配置要使用的算法的名称和类型。然后，这三个服务器会公开与其任务相对应的操作接口。当行为树勾选相应的 BT 节点时，它将调用操作服务器来处理其任务。服务器内部的操作服务器回调将通过映射FollowPath到特定算法的名称（例如）调用所选算法。

• • 下面逐个介绍对应的几个模块:
    

• Planner Server规划器：计算一条路径来完成某个目标函数。该路径也可以称为路线，具体取决于所选的命名法和算法。例如计算最短路径，计算完整覆盖路径或者沿稀疏或预定义路线计算路径。

• Controller Server控制器：(笑，问就是这是某些同学的考核任务):是追随全局计算路径或完成本地任务的方式，可以理解为局部的规划，通常要在追随全局路径的过程中对局部地图信息进行处理。

• Behavior Server行为器：recovery恢复行为作用的地方！恢复行为是容错系统的主要支柱。恢复的目标是处理系统的未知或故障情况并自主处理它们(例如机器卡死了或路径规划失败惹)

• Smoother Server平滑化器：由于Planner规划器搜索的路径最优性标准通常与现实情况相比有所降低，因此额外的路径细化通常是有益的。为此目的，引入了平滑器，通常负责减少路径粗糙度和平滑突然旋转，但也负责增加与障碍物和高成本区域的距离，因为平滑器可以访问全局环境表示。

• 足迹Footprints:机器人在代价地图中被概括的对象,通常可以设置为圆形或者多边形。

• 航点跟踪Waypoint Following：它告诉机器人如何使用导航到达多个目的地。为此，这里介绍一个nav2官方实现的航电追随功能包：

  • nav2_waypoint_follower：是一个示例应用程序，它展示了如何使用导航操作来完成某种精心安排的任务。在此示例中，该任务是获取一组给定的航点，并按照操作请求中提供的顺序导航到一组位置。航点数组中的最后一个航点是最终位置

状态估计

• 通常在导航项目中，需要提供 2 个主要转换。熟悉nav1框架的朋友应该不陌生:
  • map->odom:一般由定位系统如SLAM提供
  • odom-> 转换base_link：一般由odom里程计系统提供。
    

准备工作

• 在正式开始配置nav2前，我们再来回顾一些机器人小常识，要确保完成了以下的基础配置的流程，你才是正在具备开始nav2的前置条件。

TF坐标转换

• tf全称Transforms，定义了不同坐标系之间的关系，包括平移、旋转和相对运动。

• 这时候不得不献祭上这张陈年老图

• Nav2 需要在 ROS2 中发布以下转换：

1. map->odom(AMCL)
2. odom->base_link(里程计)
3. base_link-> base_laser（传感器基架）

• 通常我们使用tf2，也就是第二代的tf进行编写,tf2主要实现了以下模块:
  • tf2_bullet
  • tf2_eigen
  • tf2_geometry_msgs
  • tf2_kdl
  • tf2_sensor_msgs
• 通过上述模块，我们可以轻松对tf坐标信息进行广播，发布。

URDF

• 统一机器人描述格式 (URDF) 是一个表示机器人模型的 XML 文件。
• URDF 支持 Xacro(XML 宏)，帮助我们创建更短、更易读的 XML，以帮助定义复杂的机器人。我们可以使用这些宏来消除在 URDF 中重复 XML 块，重复使用的配置常量。
• 通过urdf的配置，我们可以获得可视化一个机器人的必要操作。

里程计

• 里程计系统可根据机器人的运动提供局部精确的机器人姿势和速度估计。(人话:通过传感器信息倒推机器人当前的位置及其运动学信息)
• 通常我们可以使用轮式里程计，IMU、激光雷达或者VIO 进行配置，但是考虑到单一传感器容易随着时间增加产生误差累积，我们通常使用多传感器融合来实现里程计系统。(例如robot_localization)
• 我们来看看Nav2 中关于里程计的定义nav_msgs/Odometry
  • pose：位置信息
  • twist：姿态信息

# This represents estimates of position and velocity in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id
# The twist in this message should be specified in the coordinate frame given by the child_frame_id

# Includes the frame id of the pose parent.
std_msgs/Header header

# Frame id the pose is pointing at. The twist is in this coordinate frame.
string child_frame_id

# Estimated pose that is typically relative to a fixed world frame.
geometry_msgs/PoseWithCovariance pose

# Estimated linear and angular velocity relative to child_frame_id.
geometry_msgs/TwistWithCovariance twist

• 通常一个实体机器人的里程计系统需要下位机对传感器数据进行采集后和上位机通讯后由上位机处理好数据再进行最终发布。下面是一个STM32计算差速模型里程计的公式

linear = (right_wheel_est_vel + left_wheel_est_vel) / 2;
angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation;

Gazebo

• 你是否被gazebo联网启动硬控3分钟?(乐)Gazebo是一个 3D 模拟器，让我们可以观察虚拟机器人在模拟环境中的运行情况。

• 

• gazebo除了可以加载urdf配置的机器人模型，还可以对激光雷达，轮式里程计，IMU，等传感器进行仿真，并模拟真实传感器发布对应的话题供我们使用，为此他是仿真下我们学习配置ROS2的一个不错选择

定位robot_localization

• 如上面介绍robot_localization根据里程计传感器输入提供的数据提供融合且局部准确的平滑里程计信息。
• 常见的机器人设置至少包括车轮编码器和 IMU 作为其里程计传感器源。为此robot_localization能够通过使用状态估计节点融合传感器提供的里程计信息。这些节点使用扩展卡尔曼滤波器 ( ekf_node) 或无迹卡尔曼滤波器 ( ukf_node) 来实现这种融合。此外，该包还实现了navsat_transform_node在使用 GPS 时将地理坐标转换为机器人的世界坐标系。
• 通常我们只需要配置一个ekf.yaml即可完成对robot_localization的配置

### ekf config file ###
ekf_filter_node:
    ros__parameters:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of theinputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
        frequency: 30.0

# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
        two_d_mode: false

# Whether to publish the acceleration state. Defaults to false if unspecified.
        publish_acceleration: true

# Whether to broadcast the transformation over the /tf topic. Defaultsto true if unspecified.
        publish_tf: true

# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame"
#    to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark
#    observations) then:
#     3a. Set your "world_frame" to your map_frame value
#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node
#         from robot_localization! However, that instance should *not* fuse the global data.
        map_frame: map              # Defaults to "map" if unspecified
        odom_frame: odom            # Defaults to "odom" if unspecified
        base_link_frame: base_link  # Defaults to "base_link" ifunspecified
        world_frame: odom           # Defaults to the value ofodom_frame if unspecified

        odom0: demo/odom
        odom0_config: [true,  true,  true,
                       false, false, false,
                       false, false, false,
                       false, false, true,
                       false, false, false]

        imu0: demo/imu
        imu0_config: [false, false, false,
                      true,  true,  true,
                      false, false, false,
                      false, false, false,
                      false, false, false]

全局传感器

• 上述对tf进行说明是提到了base_link-> base_laser转换所需的是一个全局传感器，常用的传感器包括激光雷达、雷达、RGB 摄像头、深度摄像头、IMU 和GPS。
• 为了同一不同来源不同类型的传感器数据，ROS2提供了sensor_msgs这个功能包用于规范不同传感器的信息，下面介绍几个常见的sensor_msgs
  • sensor_msgs/LaserScan：通常为2维雷达的信息输入，此消息用于nav2的slam_toolbox定位nav2_amcl以及建图
  • sensor_msgs/PointCloud2：此消息包含一组 3D 点，以及有关每个点的可选附加信息。这些信息可以来自 3D 激光雷达、2D 激光雷达、深度摄像头等。
  • sensor_msgs/Range：通常来自活动测距仪的单个测距读数，该测距仪发射能量并报告一个在测量距离的弧线上有效的测距读数。声纳、红外传感器或 1D 测距仪是使用此消息的传感器的示例。
  • sensor_msgs/Image：通常是来自 RGB 或深度摄像头的传感器读数，对应 RGB 或范围值。
    

建图和定位

• 在已经获得的传感器信息的前提下，我们来对机器人进行建图和定位。这里介绍两个功能包来实现：
  • slam_toolbox是一套工具和功能，用于使用 ROS2 在可能巨大的地图中进行 2D 同步定位和地图构建 (SLAM)。它也是 Nav2 中官方支持的 SLAM 库之一。
  • nav2_amcl。该软件包实现了自适应蒙特卡罗定位 (AMCL)，它可以估计机器人在地图中的位置和方向。
• 后续会详细介绍如何配置这两个参数，这里不多进行描述

2D代价地图

• costmap 2D代价地图利用传感器信息以占用网格Grid的形式提供机器人环境的表示。
• 占用网格occupancy grid中的单元格存储 0-254 之间的代价值，这些代价值表示穿越这些区域的成本。
  • 成本为 0 表示单元格是空闲的，
  • 成本为 254 表示单元格被严重占用。
• 代价地图实现由多个层组成，每个层都具有一定的功能，有助于单元的总成本。Nav2 中是通过该nav2_costmap_2d包实现的，而该包由以下层组成，但基于插件，允许自定义和使用新层：静态层、膨胀层、范围层、障碍物层和体素层
  

足迹Footprint

• 正如前面所提到的，足迹在投射到地面时勾勒出机器人的 2D 形状，主要由 Nav2 用来避免规划过程中发生碰撞。
• 对于全局代价地图覆盖范围，选择robot_radius（圆形）还是footprint（多边形）参数取决于机器人、其环境以及您将使用的路径规划算法。

导航插件

• 导航算法在 Nav2 中通过使用在 ROS 操作服务器上运行的插件（规划器、控制器和行为服务器等）来实现。正如上文提到，高度抽象和模块化设计使用户可以轻松替换其中任何一个部件，这里我们来简单介绍一下规划器和控制器的几个可替换选项。

• Planner Server

  • 默认情况下使用NavFn规划器，它是一种使用 Dijkstra或 A* 的导航功能规划器。
    

• Controller Server

  • 默认控制器插件是DWB 控制器，该控制器使用 来生成一组可能的轨迹。然后由一个或多个 来评估这些轨迹，每个轨迹可能根据其配置方式给出不同的分数。这些得分的总和决定了轨迹的总得分。然后，得分最高的轨迹决定输出命令速度。
    

总结

• 本节主要介绍了nav2入门的一些基本概念，下一节开始我们正式开始代码配置和实操
• 如有错误，欢迎指正！

参考文献

• navigtion2视频一览
• navigation2官方文档
• 动作服务器
• 生命周期节点和绑定
• tf2文档