机器人 / 系统课时
配置 BackUp、Spin 恢复行为
配置 BackUp、Spin 恢复行为
在 ROS 2 Jazzy 的 Nav2 仿真中,机器人可能因规划失败、路径跟踪受阻或代价地图残留而停住。本课把原地旋转、等待和后退接入恢复树,设置可控的运动幅度,并用障碍场景验证恢复是否安全、是否真的重新尝试导航。
学习目标
- 区分局部清图、通用恢复及导航重试的触发关系。
- 配置
Spin、Wait、BackUp的行为插件与 BT 参数。 - 在 Gazebo 和 RViz 中验证动作完成、碰撞拒绝及恢复失败。
识别恢复触发顺序
在 Jazzy 的默认 NavigateToPose 行为树中,ComputePathToPose 规划失败时,先根据错误码判断是否有必要清除全局代价地图;FollowPath 失败时,对应尝试清除局部代价地图。这两处是上下文恢复,并不意味着立刻后退。
导航主分支仍失败、且 WouldAPlannerRecoveryHelp 或 WouldAControllerRecoveryHelp 判断可恢复时,外层 NavigateRecovery 才进入通用恢复。其 RoundRobin 按默认顺序尝试清除局部和全局代价地图、Spin、Wait、BackUp。某项恢复成功后,外层会重新尝试规划与跟踪;反复失败则继续尝试其他恢复,直至达到重试上限。Jazzy 默认外层 number_of_retries 为 6。

这三项的用途不同:Spin 用于尝试改变朝向,Wait 适合短暂移动障碍可能离开的情形,BackUp 则在后方有可用空间时为重新规划腾出距离。Wait 不检测障碍是否消失;它只是等待指定时间后把控制权交回树。
启用行为插件
在当前使用的 nav2_params.yaml 中检查 behavior_server.ros__parameters。下面是仅展示本课三项插件的片段示例;已有 drive_on_heading、assisted_teleop 等插件时,应保留原列表和对应配置,不要直接覆盖掉它们。
behavior_server:
ros__parameters:
behavior_plugins: ["spin", "backup", "wait"]
spin:
plugin: "nav2_behaviors::Spin"
backup:
plugin: "nav2_behaviors::BackUp"
wait:
plugin: "nav2_behaviors::Wait"
simulate_ahead_time: 2.0
max_rotational_vel: 0.6
min_rotational_vel: 0.2
rotational_acc_lim: 1.0simulate_ahead_time 设为 2.0 秒,用于行为执行时前瞻碰撞检查;三个旋转速度与加速度参数单位依次为 rad/s、rad/s、rad/s²。此处用较温和的旋转限制做差速底盘仿真起点,后续须按机器人能力调整。Jazzy 插件类型使用 ::,不要照搬较早版本的 / 写法。旋转角度、后退距离和等待时长由 BT 节点传入,不是这里的服务器参数。
用原有启动方式加载参数文件并激活 Nav2 后,运行:
source /opt/ros/jazzy/setup.bash
ros2 lifecycle get /behavior_server
ros2 action list -t | grep -E '^/(backup|spin|wait) '预期输出(无命名空间的标准启动,顺序可能不同):
active [3]
/backup [nav2_msgs/action/BackUp]
/spin [nav2_msgs/action/Spin]
/wait [nav2_msgs/action/Wait]如果动作名称带机器人命名空间,检查对应完整名称;没有这些 Action 时,先确认 /behavior_server 是否已激活、插件配置是否被当前 Launch 正确加载。
修改恢复行为树
复制已安装的 Jazzy 默认 XML 到可编辑位置,避免直接修改系统包:
cp "$(ros2 pkg prefix --share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml" "$HOME/nav2_recovery.xml"
test -s "$HOME/nav2_recovery.xml" && echo "BT XML 已复制"预期输出:
BT XML 已复制打开 ~/nav2_recovery.xml,在原有 ReactiveFallback 内仅替换 RoundRobin name="RecoveryActions" 这一子树,保留外层 RecoveryNode、GoalUpdated 和错误判断。下面是必须嵌入原树的片段示例,不能单独作为完整 BT 文件加载:
<RoundRobin name="RecoveryActions">
<Sequence name="ClearingActions">
<ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<!-- 先试旋转与等待,再尝试小幅后退 -->
<Spin spin_dist="1.57" time_allowance="10.0"/>
<Wait wait_duration="5.0"/>
<BackUp backup_dist="0.15" backup_speed="0.05"
time_allowance="10.0"/>
</RoundRobin>1.57 rad 约为 90°;后退 0.15 m、速度 0.05 m/s,理想匀速行驶约需 3 秒,实际还受控制周期和减速影响。两项 time_allowance="10.0" 单位为秒,是行为超时预算,不是固定运动时间。Wait 的 5.0 秒仅暂停当前恢复步骤。
在启动参数的 bt_navigator.ros__parameters.default_nav_to_pose_bt_xml 中填写该 XML 的绝对路径,再重新启动 Nav2。可使用 realpath ~/nav2_recovery.xml 查询路径;YAML 中不要直接填 ~。参照 Jazzy 默认恢复树 核对外层结构。
在仿真中验证动作与自动恢复
先在 Gazebo 把机器人放在前后、转向均无遮挡的区域,停止当前导航目标。可直接向 Behavior Server 发送三个独立的 Action 目标,以确认插件本身能运行:
ros2 action send_goal /spin nav2_msgs/action/Spin "{target_yaw: 1.57, time_allowance: {sec: 10, nanosec: 0}}" | grep 'Goal finished with status'
ros2 action send_goal /backup nav2_msgs/action/BackUp "{target: {x: 0.15, y: 0.0, z: 0.0}, speed: 0.05, time_allowance: {sec: 10, nanosec: 0}}" | grep 'Goal finished with status'
ros2 action send_goal /wait nav2_msgs/action/Wait "{time: {sec: 5, nanosec: 0}}" | grep 'Goal finished with status'预期输出(各动作均获准且环境安全时):
Goal finished with status: SUCCEEDED
Goal finished with status: SUCCEEDED
Goal finished with status: SUCCEEDED这只能证明三个 Action 可运行,不能证明 BT 会自动触发它们。再用 RViz 的 2D Goal Pose 向一段可导航通道后方发送目标,在 Gazebo 中放入可移走的临时障碍,使规划或跟踪确实失败;对照代价地图、机器人运动、/behavior_server 日志,检查是否先清图、恢复完成后是否重新导航。各动作是否被轮到,取决于错误码、恢复状态和此前动作的结果,不要求每次都依次执行三项。
最后让后退方向被障碍占据,或让旋转扫过障碍,重复测试。期望结果是碰撞检查阻止不安全的运动,而不是强行完成后退或旋转。如果动作持续失败,优先检查局部代价地图的障碍和 footprint、odom 到 base_link 的 TF、底盘能否执行负线速度,以及速度话题是否经过限速或安全过滤;不要靠增大 number_of_retries 或关闭碰撞检查掩盖问题。