机器人 / 系统课时

用状态机组织导航与失败恢复

用状态机组织导航与失败恢复

机器人接到目标后,可能因路径不可达、障碍阻挡或定位异常而失败。如果上层收到失败就无限重发目标,底盘会反复尝试同一条不可行路线。本篇用有限状态机包装一次 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 文档。

python
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 自身日志):

text
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,并记录目标坐标、派发次数、动作结果和故障原因,避免旧结果污染新任务。

验证状态机不会无限循环

先在仿真或封闭测试区域执行,按以下条件验收:

  1. 发送一个可达目标,观察 state=SUCCEEDED,确认最多一次派发且机器人在目标容差内停止。
  2. 发送不可达目标:如果 Nav2 返回 FAILED,最多出现两次 NAVIGATING 和一次 RECOVERING;如果持续执行超过 120 秒,应进入取消处理分支,不能自动发第二次。
  3. 人工取消导航或制造临时遮挡,确认 CANCELED 不触发重试;仅明确的 FAILED 才可能进入 RECOVERING。
  4. 记录每次状态迁移的时间、尝试序号及最终原因,并检查 CANCEL_REQUESTED 是否触发人工或安全监控,而不是被误判为成功。

这些终态只说明任务管理器正确处理了 Nav2 动作结果。实际部署还需要用里程计、碰撞监测与底盘安全机制独立确认机器人停止,不能把程序退出当作停车证明。

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

目录与阅读设置

阅读设置

17

记录一个想法

用状态机组织导航与失败恢复

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

登录哪都通