机器人 / 系统课时
用状态机组织导航与失败恢复
用状态机组织导航与失败恢复
机器人接到目标后,可能因路径不可达、障碍阻挡或定位异常而失败。如果上层收到失败就无限重发目标,底盘会反复尝试同一条不可行路线。本篇用有限状态机包装一次 Nav2 导航任务:限定执行时间和尝试次数,在可恢复时只重试一次,最终给出可检查的终态。
学习目标
- 区分 Nav2 内部行为树恢复与任务级状态管理的职责。
- 设计包含成功、失败、取消和有限重试的状态迁移。
- 用 ROS 2 Jazzy 的 Python API 运行并检查一次导航任务。
明确状态和转换条件
Nav2 的 NavigateToPose 动作内部会使用行为树进行规划、跟踪路径和局部恢复。上层不应把控制器的一次临时失败直接当成整项任务失败,而应等待导航动作返回结果,再决定是否重新派发目标。这里的状态是应用程序自定义的,不是 Nav2 自带的状态枚举。
| 任务状态 | 进入条件 | 允许的下一状态 |
|---|---|---|
IDLE 待命 |
目标尚未派发 | NAVIGATING |
NAVIGATING 导航中 |
目标已被动作服务器接受 | SUCCEEDED、RECOVERING、FAILED、CANCELED、CANCEL_REQUESTED |
RECOVERING 恢复中 |
首次导航返回 FAILED |
NAVIGATING(仅再试一次) |
SUCCEEDED 成功 |
TaskResult.SUCCEEDED |
终态 |
FAILED 失败 |
目标被拒绝、结果异常或重试耗尽 | 终态 |
CANCELED 已取消 |
动作结果为 TaskResult.CANCELED |
终态 |
CANCEL_REQUESTED 待确认取消 |
超时且取消未在限定时间内确认 | 停止派发,交由监控处理 |

为失败设定有界恢复规则
在单个目标任务中,先采用一套易于验证的策略:MAX_ATTEMPTS = 2 表示最多派发两次目标,不是重试两次;每次动作最多运行 120 秒;第一次收到明确的 FAILED 结果后,等待 5 秒,让临时遮挡有机会消失,然后只重发一次。第二次失败直接结束。
必须区分三种信号:FAILED 允许进入这次受限的恢复;CANCELED 代表已取消,不得自动重试;运行超过 120 秒则发出 cancelTask(),即使取消未确认也不再发送新目标。超时属于任务策略,不等同于 Nav2 报告了规划失败。
用 Python 实现有限重试
适用环境为 ROS 2 Jazzy、已启动的 Nav2 与 nav2_simple_commander。在 RViz 2 设置好 AMCL 初始位姿,确认地图中的 (1.0, 0.0) 是可达的安全位置;waitUntilNav2Active() 在 Nav2 或初始定位未就绪时会等待,不属于本例的 120 秒执行超时。将代码保存为 bounded_nav.py,在已加载 ROS 2 环境的终端运行 python3 bounded_nav.py。接口名称可对照 Nav2 Jazzy Simple Commander 文档。
import time
from enum import Enum
import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
class State(Enum):
IDLE = "IDLE"
NAVIGATING = "NAVIGATING"
RECOVERING = "RECOVERING"
SUCCEEDED = "SUCCEEDED"
FAILED = "FAILED"
CANCELED = "CANCELED"
CANCEL_REQUESTED = "CANCEL_REQUESTED"
MAX_ATTEMPTS = 2
TIMEOUT_S = 120
RECOVERY_WAIT_S = 5
def make_goal(nav):
goal = PoseStamped()
goal.header.frame_id = "map"
goal.header.stamp = nav.get_clock().now().to_msg()
goal.pose.position.x = 1.0
goal.pose.position.y = 0.0
goal.pose.orientation.w = 1.0 # 朝向 0 rad 的单位四元数
return goal
def wait_cancel_result(nav):
deadline = time.monotonic() + 5.0
while time.monotonic() < deadline:
if nav.isTaskComplete():
return nav.getResult()
return None # 未收到终态,不能假定取消成功
def main():
rclpy.init()
nav = BasicNavigator()
state = State.IDLE
try:
nav.waitUntilNav2Active()
print(f"state={state.value}")
for attempt in range(1, MAX_ATTEMPTS + 1):
state = State.NAVIGATING
print(f"state={state.value} attempt={attempt}/{MAX_ATTEMPTS}")
if not nav.goToPose(make_goal(nav)):
state = State.FAILED
print("state=FAILED reason=goal_rejected")
break
started = time.monotonic()
timed_out = False
while not nav.isTaskComplete():
if time.monotonic() - started >= TIMEOUT_S:
timed_out = True
nav.cancelTask() # 请求取消,禁止继续派发
break
if timed_out:
result = wait_cancel_result(nav)
if result == TaskResult.CANCELED:
state = State.CANCELED
elif result == TaskResult.SUCCEEDED:
state = State.SUCCEEDED
elif result is None:
state = State.CANCEL_REQUESTED
else:
state = State.FAILED
print(f"state={state.value} reason=timeout")
break
result = nav.getResult()
if result == TaskResult.SUCCEEDED:
state = State.SUCCEEDED
print("state=SUCCEEDED")
break
if result == TaskResult.CANCELED:
state = State.CANCELED
print("state=CANCELED")
break
if result != TaskResult.FAILED or attempt == MAX_ATTEMPTS:
state = State.FAILED
print("state=FAILED reason=unknown_or_attempts_exhausted")
break
state = State.RECOVERING
print(f"state={state.value} wait={RECOVERY_WAIT_S}s")
time.sleep(RECOVERY_WAIT_S) # 仅针对可能暂时阻塞的目标等待一次
finally:
nav.destroyNode()
rclpy.shutdown()
if __name__ == "__main__":
main()预期输出(目标可达且第一次成功时,省略 Nav2 自身日志):
state=IDLE
state=NAVIGATING attempt=1/2
state=SUCCEEDED如果第一次导航失败而第二次仍失败,程序依次打印 RECOVERING、第二次 NAVIGATING 和最终 FAILED,不会出现第三次派发。代码使用单调时钟计时,避免系统时间调整改变超时判断;每次重发都会重新生成目标时间戳。
根据故障类型调整恢复动作
上面的 5 秒等待只适合人员暂时占道等短时阻塞。Nav2 内部行为树通常已经执行过清理代价图、等待等恢复,因此上层不能每逢失败就再次盲目清图或原地旋转。
| 观察现象 | 任务级处理 |
|---|---|
行人短暂挡路,导航动作最终返回 FAILED |
允许等待 5 秒,再试一次 |
| 目标落在静态障碍或地图外,连续无法规划 | 标记 FAILED,修改目标位置后创建新任务 |
| RViz 显示已移走的障碍仍留在局部代价图 | 先检查传感器清除观测和 TF;确认是过期数据后,才考虑 clearLocalCostmap() |
/map 定位跳变、TF 不可用或传感器失效 |
停止任务并修复定位/时间同步,不自动重复发目标 |
| 人工取消或安全系统要求停车 | 停止派发,并确认底盘实际停止 |
clearLocalCostmap() 是 Jazzy Simple Commander 的真实 API,但清除代价图不会移除物理障碍;示例故意不在每次失败后自动调用它。生产任务还应为目标附加独立任务 ID,并记录目标坐标、派发次数、动作结果和故障原因,避免旧结果污染新任务。
验证状态机不会无限循环
先在仿真或封闭测试区域执行,按以下条件验收:
- 发送一个可达目标,观察
state=SUCCEEDED,确认最多一次派发且机器人在目标容差内停止。 - 发送不可达目标:如果 Nav2 返回
FAILED,最多出现两次NAVIGATING和一次RECOVERING;如果持续执行超过 120 秒,应进入取消处理分支,不能自动发第二次。 - 人工取消导航或制造临时遮挡,确认
CANCELED不触发重试;仅明确的FAILED才可能进入RECOVERING。 - 记录每次状态迁移的时间、尝试序号及最终原因,并检查
CANCEL_REQUESTED是否触发人工或安全监控,而不是被误判为成功。
这些终态只说明任务管理器正确处理了 Nav2 动作结果。实际部署还需要用里程计、碰撞监测与底盘安全机制独立确认机器人停止,不能把程序退出当作停车证明。