有限狀態機（Finite State Machine）

1.有限狀態機（Finite State Machine）

FSM就是：把任務分成幾個狀態，並規定遇到什麼事件時切換狀態。

重點理解四件事：

• 狀態： 當前任務在做什麼，例如“前往目標”。
• 事件／條件： 發生了什麼，例如導航返回成功。
• 轉移： 從“前往目標”切到“到點等待”。
• 進入／退出動作： 進入導航狀態時傳送一次目標；中途退出時取消尚未完成的目標。 狀態機通常圍繞“當前處於哪個狀態，什麼時候切換”組織邏輯；BT圍繞條件檢查和動作組合組織邏輯。兩者都能做任務決策。

1.1.例子

1.1.1.例子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);
}

核心就是：先看當前狀態，再看收到的事件，決定是否切換狀態以及執行什麼動作。

1.1.2.例子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