4. 实现选择节点处理导航失败
任务目标
在ROS2 Nav2导航任务中，当主目标点导航失败时，使用行为树中的Fallback（回退/选择）节点自动切换至备用目标点继续尝试，并在切换过程中记录失败原因。本案例将构建一个最小可复现的XML行为树，并通过Nav2的bt_navigator加载运行。
核心实现
Fallback节点（XML中为Fallback）按从左到右顺序执行子节点：若当前子节点返回SUCCESS则整个Fallback返回SUCCESS并停止；若返回FAILURE则继续执行下一个子节点；若全部失败则返回FAILURE。本案例利用该特性串联两个NavigateToPose动作节点，并在每个动作前插入Script节点记录当前尝试的目标点。
以下为完整行为树XML文件navigate_with_fallback.xml：
运行与验证
依赖环境：已安装ROS2 Humble或更高版本、Nav2栈（含bt_navigator）、以及一个可用的仿真或真实机器人平台（如turtlebot3_gazebo）。
配置步骤：
- 将上述XML保存至
~/nav2_ws/src/nav2_bringup/behavior_trees/目录下（或你的Nav2行为树配置目录）。 - 在Nav2参数文件（如
nav2_params.yaml）中设置：bt_navigator: ros__parameters: default_bt_xml_filename: "navigate_with_fallback.xml" - 启动Nav2导航栈和机器人仿真环境。
验证方法：
- 通过RViz2的“2D Goal Pose”工具发布一个不可达的主目标点（如地图障碍物内部），再发布一个可达的备用目标点。
- 观察机器人行为：应首先尝试前往主目标，失败后自动转向备用目标并成功到达。
- 在终端执行以下命令查看黑板变量，确认失败原因已记录：
或通过ros2 run nav2_bt_navigator bt_navigator --show-blackboardros2 topic echo /bt_navigator/blackboard（若已配置发布）查看reason字段值为primary_failed。
通过判定标准：
- 机器人先向主目标移动，在无法到达后（Nav2动作返回
FAILURE）自动切换至备用目标并成功到达。 - 黑板上记录的
reason变量为primary_failed，证明Fallback分支切换逻辑正确执行。
关键说明
- 本案例中
NavigateToPose的server_timeout设为10秒，表示若10秒内未收到动作服务器响应则视为失败。实际超时时间可根据导航场景调整。 - 黑板上
primary_goal和backup_goal需由外部节点（如任务调度器）在行为树执行前设置，本案例聚焦Fallback逻辑本身，故未包含目标点赋值节点。 - 若需记录更详细的失败原因（如“规划失败”或“控制超时”），可在
NavigateToPose后添加CheckBlackboard或Switch节点读取动作结果消息，本案例以最简方式演示核心回退机制。