有限状态机（Finite State Machine）

有限状态机（Finite State Machine）

FSM就是：把任务分成几个状态，并规定遇到什么事件时切换状态。

重点理解四件事：

• 状态： 当前任务在做什么，例如“前往目标”。
• 事件／条件： 发生了什么，例如导航返回成功。
• 转移： 从“前往目标”切到“到点等待”。
• 进入／退出动作： 进入导航状态时发送一次目标；中途退出时取消尚未完成的目标。 状态机通常围绕“当前处于哪个状态，什么时候切换”组织逻辑；BT围绕条件检查和动作组合组织逻辑。两者都能做任务决策。

例子

例子1

做一个小例子：

首先定义四个状态：

再定义事件：开始任务、导航成功、导航失败、重置。

状态是“现在处于什么阶段”，事件是“刚刚发生了什么”。

状态机根据这两项决定下一步。

// 定义任务可能处于的状态
enum class State {
    IDLE,        // 待机：还没有开始任务
    NAVIGATING,  // 导航中：已经发送目标，正在等待结果
    COMPLETED,   // 已完成：导航成功
    ERROR        // 异常：导航失败
};

// 定义可能收到的事件
enum class Event {
    START,          // 收到“开始任务”指令
    NAV_SUCCEEDED,  // 导航模块报告成功
    NAV_FAILED,     // 导航模块报告失败
    RESET           // 收到“重置任务”指令
};

// 保存当前状态，初始状态为待机
State current_state = State::IDLE;

// 函数声明：暂时代表发送A点目标，具体实现以后再写
void sendGoalA();

// 每次发生事件时，调用这个函数
void handleEvent(Event event)
{
    // 先判断任务当前处于哪个状态
    switch (current_state) 
    {

        case State::IDLE:
            // 待机状态下，收到开始指令才启动导航
            if (event == Event::START) 
            {
                current_state = State::NAVIGATING;
                sendGoalA();  // 发送一次导航目标
            }
            break;  // 跳出switch，本次事件处理结束

        case State::NAVIGATING:
            // 导航中，根据导航结果切换状态
            if (event == Event::NAV_SUCCEEDED) 
            {
                current_state = State::COMPLETED;
            } else if (event == Event::NAV_FAILED) 
            {
                current_state = State::ERROR;
            }

            // 如果收到START等其他事件，保持当前状态
            break;

        // 已完成和异常状态，共用下面的重置逻辑
        case State::COMPLETED:
        case State::ERROR:
            if (event == Event::RESET) 
            {
                current_state = State::IDLE;
            }
            break;
    }
}

下面模拟事件依次发生：

void demo()
{
    // 当前为待机，操作员点击开始指令：进入导航中，发送目标
    handleEvent(Event::START);

    // 当前已在导航中，再收到开始指令：忽略，不重复发送目标
    handleEvent(Event::START);

    // NAV2导航报告成功：进入已完成状态
    handleEvent(Event::NAV_SUCCEEDED);

    // 收到重置指令：回到待机状态
    handleEvent(Event::RESET);
}

核心就是：先看当前状态，再看收到的事件，决定是否切换状态以及执行什么动作。

例子2

增加“到点后等待2秒”的功能：

#include <chrono>

using Clock = std::chrono::steady_clock;

// 任务状态
enum class State {
    IDLE,        // 待机
    NAVIGATING,  // 导航中
    WAITING,     // 到点后等待
    COMPLETED,   // 任务完成
    ERROR        // 任务异常
};

// 任务事件
enum class Event {
    START,          // 开始任务
    NAV_SUCCEEDED,  // 导航成功
    NAV_FAILED,     // 导航失败
    WAIT_FINISHED,  // 等待结束
    RESET           // 重置任务
};

State current_state = State::IDLE;

// 保存等待的结束时刻
Clock::time_point wait_deadline;

// 暂时代表发送导航目标，具体实现以后再写
void sendGoalA();

// 处理事件
void handleEvent(Event event)
{
    switch (current_state) {
        case State::IDLE:
            if (event == Event::START) 
            {
                current_state = State::NAVIGATING;
                sendGoalA();  // 只发送一次目标
            }
            break;

        case State::NAVIGATING:
            if (event == Event::NAV_SUCCEEDED) 
            {
                // 导航成功时，设置一次等待结束时刻
                wait_deadline =
                    Clock::now() + std::chrono::seconds(2);

                current_state = State::WAITING;
            } 
            else if (event == Event::NAV_FAILED) 
            {
                current_state = State::ERROR;
            }
            break;

        case State::WAITING:
            if (event == Event::WAIT_FINISHED) 
            {
                current_state = State::COMPLETED;
            }
            break;

        case State::COMPLETED:
        case State::ERROR:
            if (event == Event::RESET) 
            {
                current_state = State::IDLE;
            }
            break;
    }
}

// 主循环或定时器周期性调用这个函数
void tick()
{
    // 只有处于等待状态，才检查时间
    if (current_state == State::WAITING) 
    {
        if (Clock::now() >= wait_deadline) 
        {
            handleEvent(Event::WAIT_FINISHED);
        }
    }
}

模拟调用过程：

handleEvent(Event::START);          // 待机 → 导航中
handleEvent(Event::NAV_SUCCEEDED);  // 导航中 → 等待，启动一次计时

// 接下来，主循环周期性调用tick()
tick();  // 未到结束时刻：保持WAITING
tick();  // 未到结束时刻：保持WAITING

// 2秒到期后的下一次检查
tick();  // 触发WAIT_FINISHED，进入COMPLETED