📚 课程目录第3章实现顺序节点控制导航序列

3. 实现顺序节点控制导航序列

任务目标

在ROS2 Nav2导航任务中，经常需要机器人依次访问多个目标点（如巡检路线）。本案例使用BehaviorTree.CPP中的Sequence节点（顺序节点）控制导航序列：只有当前导航目标成功到达后，才继续执行下一个目标；任一目标失败则整棵行为树返回失败并停止后续导航。

核心实现

本案例基于Nav2的bt_navigator组件，通过自定义行为树XML实现多目标点顺序导航。Sequence节点（Sequence）从左到右执行子节点，子节点返回SUCCESS后才执行下一个；若某子节点返回FAILURE，则整棵序列立即返回FAILURE。

需要准备：

ROS2 Humble或更高版本，已安装nav2_bringup、navigation2、behavior_tree包
一个仿真环境（如turtlebot3_gazebo）或真实机器人
三个目标点坐标（示例中为goal_pose数组）

行为树XML文件

<?xml version="1.0"?>
<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Sequence name="nav_sequence">
      <NavigateToPose goal="{goal1}"/>
      <NavigateToPose goal="{goal2}"/>
      <NavigateToPose goal="{goal3}"/>
    </Sequence>
  </BehaviorTree>
</root>

导航目标点配置

在Nav2的bt_navigator参数中，需要将上述XML文件路径配置到default_bt_xml_filename，并通过ROS2话题或参数服务器提供goal1、goal2、goal3的坐标。以下为最小配置示例（nav2_params.yaml片段）：

bt_navigator:
  ros__parameters:
    default_bt_xml_filename: "path/to/sequence_nav.xml"
    goal_blackboard:
      goal1: {x: 1.0, y: 1.0, yaw: 0.0}
      goal2: {x: 2.0, y: 2.0, yaw: 1.57}
      goal3: {x: 3.0, y: 1.0, yaw: 3.14}

运行与验证

启动仿真环境：ros2 launch nav2_bringup tb3_simulation_launch.py
启动Nav2导航栈（加载上述参数文件）
通过RViz2的“Nav2 Goal”工具发布第一个目标点，或直接调用/bt_navigator/navigate_to_pose服务

验证方法：

观察RViz2中机器人路径：机器人应依次经过三个目标点，且到达第一个点后才开始向第二个点移动
在终端执行ros2 topic echo /bt_navigator/transition_event，可看到行为树状态切换：READY→RUNNING→SUCCESS依次对应每个导航目标
若人为在第一个目标点前方放置障碍物导致导航失败，行为树应停止执行后续目标，并返回FAILURE状态

通过条件：机器人按顺序到达全部三个目标点，且行为树最终状态为SUCCESS；若中途任一目标失败，行为树立即终止且不再执行后续导航。

关键说明

Sequence节点的核心价值在于短路机制：一旦某子节点失败，后续节点不再执行，这避免了“目标1未到达却继续尝试目标2”的逻辑错误
实际工程中可将goal1~goal3替换为从参数服务器或外部话题动态读取的目标点数组，但本案例保持最小可复现实现
若需循环执行整个序列，可在Sequence外层包裹Repeat节点，但这不是本案例目标