机器人 / 系统课时

读取 BT Navigator 执行树

读取 BT Navigator 执行树

一次 NavigateToPose 导航并非只规划一次路径。以 ROS 2 Jazzy 的默认行为树为样本,本课带你找到 XML,沿父子节点读懂规划、跟踪与恢复的执行条件,并在仿真中验证判断,避免把所有导航失败都归因于规划器。

学习目标

  • 定位 Jazzy 的默认导航行为树并核对关键节点。
  • 根据 SUCCESS、FAILURE、RUNNING 判断 Sequence 与 Fallback 的执行分支。
  • 找出路径黑板、条件检查和分层恢复之间的联系。

定位实际使用的 XML

适用环境为 Ubuntu 24.04、ROS 2 Jazzy,已安装 nav2_bt_navigator。默认 NavigateToPose 树文件名是 navigate_to_pose_w_replanning_and_recovery.xml。执行下面的完整示例,可从已安装软件包读取 XML,统计主要控制、动作与条件节点。

bash
source /opt/ros/jazzy/setup.bash
# 使用 ROS 包前缀定位 XML,避免写死工作区路径
BT="$(ros2 pkg prefix nav2_bt_navigator)/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"
python3 - "$BT" <<'PY'
import sys
import xml.etree.ElementTree as ET

root = ET.parse(sys.argv[1]).getroot()
# 用节点数量核对是否读到 Jazzy 的默认树
for tag in ("RecoveryNode", "PipelineSequence", "Sequence", "Fallback",
            "ReactiveFallback", "RateController", "ComputePathToPose",
            "FollowPath", "GoalUpdated"):
    print(f"{tag}: {len(root.findall('.//' + tag))}")
PY

预期输出(官方 Jazzy 分支的该文件):

text
RecoveryNode: 3
PipelineSequence: 1
Sequence: 4
Fallback: 1
ReactiveFallback: 1
RateController: 1
ComputePathToPose: 1
FollowPath: 1
GoalUpdated: 1

在已启动的导航系统中,还应检查 ros2 param get /bt_navigator default_nav_to_pose_bt_xml:参数为空时使用软件包默认树;非空时应读取指定文件。单次 NavigateToPose 动作目标也能通过 behavior_tree 字段选择另一棵树,不能仅凭文件名认定实际运行的是默认树。可对照官方 Jazzy 版本 XML。

按返回状态读控制节点

行为树按 tick 执行;叶节点返回 SUCCESS、FAILURE 或 RUNNING。遇到父节点时,先看它怎样处理子节点状态,再判断哪一支会运行。

控制节点 执行规则 本树中的用途
Sequence 依次执行;遇 FAILURE 停止,遇 RUNNING 等待,全部成功才成功 先判断错误是否值得恢复,再清理代价地图
Fallback 从左到右尝试;只在前一子节点失败时试下一项 在两种错误可恢复条件中择一
PipelineSequence 后续节点运行期间持续重新 tick 前面的节点 一边跟踪路径,一边定期重规划
RecoveryNode 主子树失败后执行恢复子树,恢复成功则重试主子树 规划、控制的局部恢复和导航总体恢复

跟踪规划结果如何传递

树顶是 RecoveryNode name="NavigateRecovery";其导航主分支为 PipelineSequence name="NavigateWithReplanning"。先由 ControllerSelector 和 PlannerSelector 选择插件,再由 RateController hz="1.0" 控制规划请求节奏,最后执行路径跟踪。

Jazzy 默认导航行为树中规划、跟踪和恢复节点的父子层级

读主链时抓住三个端口:ComputePathToPose 从黑板读取 {goal},把全局路径写入 {path};FollowPath 再读取同一个 {path},并按选定控制器运行。RateController 标明的是规划分支约 1 Hz 的触发节奏,不是底盘控制频率;路径有更新时,跟踪动作可以收到新路径。

用条件节点解释失败分支

如果规划失败,内部 RecoveryNode number_of_retries="1" 会先运行 WouldAPlannerRecoveryHelp。条件为成功时才调用 ClearEntireCostmap 清全局代价地图,然后重试规划。控制失败则检查 WouldAControllerRecoveryHelp,通过后清局部代价地图并重试跟踪。条件不成立时,不会无条件清图。

当主导航分支仍失败,顶层恢复 Sequence 先用 Fallback 检查规划或控制错误是否适合恢复;两项都失败就不会进入后面的通用恢复。通过检查后,ReactiveFallback 优先检查 GoalUpdated;没有新目标可打断旧恢复时,RoundRobin 依次尝试清两张代价地图、旋转 1.57 rad、等待 5.0 s、后退 0.30 m(速度 0.15 m/s)。顶层 number_of_retries="6" 约束导航恢复后的重试次数,不表示六个恢复动作同时执行。

在仿真中核对读树结论

  1. 启动已配置好地图、定位、代价地图和 Nav2 的 Jazzy 仿真,在 RViz 2 用 Nav2 Goal 选一个可达空闲位置。观察规划路径出现、机器人沿路径运动;这对应 ComputePathToPose 先提供路径、FollowPath 持续运行。
  2. 再尝试一个被连续障碍隔开的不可达目标,观察 bt_navigator 及规划器、控制器日志。记录失败发生在规划还是跟踪,再按 WouldA...RecoveryHelp 的结果检查是否应进入清图或通用恢复。
  3. 若现象与 XML 不符,先核对导航任务有没有指定自定义 behavior_tree,再确认 default_nav_to_pose_bt_xml 与实际启动参数,最后检查行为树节点插件是否加载成功。

读树时应始终区分:Sequence 决定前置条件能否继续,Fallback 决定备选条件,PipelineSequence 维持重规划与跟踪协作,而 GoalUpdated 决定是否继续处理旧目标的恢复。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

读取 BT Navigator 执行树

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通