一、FSM 的基本概念
FSM 是 Finite State Machine，有限状态机。
在机器人任务中，FSM 用来描述机器人处于哪个任务阶段，以及收到事件后应该切换到哪个状态。
核心公式可以理解为：
当前状态 + 输入事件 = 下一个状态
例如机器人执行"寻找目标并靠近"的任务时，不应该一直发布同一个速度，而是要经历：
空闲 → 准备 → 搜索 → 靠近 → 停止
FSM 的价值是把复杂任务拆成清晰状态，避免控制逻辑写成一堆混乱的 if-else。
二、状态和事件的区别
FSM 中最重要的是区分 状态 state 和 事件 event。
|--------|-------------|-------------------------------------------------|
| 概念 | 含义 | 项目例子 |
| 状态 | 机器人当前处于什么阶段 | IDLE、SEARCH、APPROACH、ERROR |
| 事件 | 触发状态变化的输入 | start、target_found、target_reached、error |
状态表示"现在是什么"。 事件表示"发生了什么"。
在我的项目里：
|---------------|-------------|
| topic | 含义 |
| /task/event | 输入给 FSM 的事件 |
| /task/state | FSM 输出的当前状态 |
所以 /task/event 和 /task/state 不能混用。前者是触发条件，后者是任务结果。
三、FSM 状态设计
我项目中设计了 7 个状态。
|------------|----------|--------------|
| 状态 | 中文含义 | 任务含义 |
| IDLE | 空闲 | 等待任务开始 |
| STAND | 站立准备 | 模拟机器人进入准备姿态 |
| SEARCH | 搜索 | 原地旋转寻找目标 |
| APPROACH | 靠近 | 发现目标后向目标靠近 |
| STOP | 停止 | 到达目标后停止 |
| REPORT | 汇报 | 可扩展为任务完成上报 |
| ERROR | 异常 | 安全保护触发后的异常状态 |
主状态转移关系：
|------------|------------------|------------|
| 当前状态 | 事件 | 下一个状态 |
| IDLE | start | STAND |
| STAND | stand_done | SEARCH |
| SEARCH | target_found | APPROACH |
| APPROACH | target_reached | STOP |
异常转移关系：
|----------|---------|-----------|
| 当前状态 | 事件 | 下一个状态 |
| 任意运行状态 | error | ERROR |
| ERROR | reset | IDLE |
注意：stand_done 只在 STAND 状态下有效。如果机器人还在 IDLE，直接发送 stand_done 被忽略是正常现象。
四、task_fsm_node 的职责
task_fsm_node 是状态机节点。
它只负责一件事：
接收任务事件，更新任务状态，发布当前状态。
|--------|------------------------------------------------------------------------------------|
| 项目 | 内容 |
| 订阅 | /task/event |
| 发布 | /task/state |
| 输入 | start、stand_done、target_found、target_reached、target_lost、error、reset |
| 输出 | IDLE、STAND、SEARCH、APPROACH、STOP、REPORT、ERROR |
它不直接发布 /cmd_vel。
这样设计的核心思想是：
FSM 负责"任务决策"，不负责"底层运动"。
也就是说：
|----------------------|------------------|
| 节点 | 负责问题 |
| task_fsm_node | 机器人现在应该处于哪个任务阶段 |
| task_motion_node | 这个状态下应该怎么动 |
| robot_control_node | 如何把速度交给底层 driver |
| MockDogDriver | 如何模拟机器人执行 |
这种分层让后续升级更容易。比如以后把 APPROACH 从固定前进改成视觉跟随，不需要重写 FSM。
五、视觉桥接节点的作用
视觉节点输出的是感知结果，例如是否检测到目标、置信度、目标框大小、目标位置等。
但 FSM 不应该直接处理复杂视觉字段。
所以需要 vision_fsm_bridge_node，它的作用是：
把视觉检测结果转换成 FSM 能理解的任务事件。
|----------------------|-------------|
| 输入 | 作用 |
| /perception/target | 接收目标检测结果 |
| /task/state | 判断当前 FSM 状态 |
| /task/event | 发布任务事件 |
核心逻辑：
|------------|----------|------------------|
| 当前状态 | 视觉条件 | 发布事件 |
| SEARCH | 检测到有效目标 | target_found |
| APPROACH | 目标面积足够大 | target_reached |
| APPROACH | 长时间没看到目标 | target_lost |
这样视觉层不会直接控制机器人，而是通过事件影响 FSM，再由 FSM 影响运动。
六、目标检测字段与任务事件
/perception/target 中常见字段如下。
|--------------|------------|--------------|
| 字段 | 含义 | 用途 |
| detected | 是否检测到目标 | 判断有没有目标 |
| confidence | 检测置信度 | 过滤不可靠检测 |
| area_ratio | 目标框面积占图像比例 | 判断目标是否足够近 |
| center_x | 目标中心横坐标 | 可用于左右方向判断 |
| offset_x | 目标相对中心的偏移 | 可用于转向控制 |
| class_name | 目标类别 | YOLO 中区分物体类别 |
| frame_id | 坐标系名称 | 后续结合 TF 使用 |
三个核心事件：
|------------------|-----------|----------------------|
| 事件 | 含义 | 常见状态变化 |
| target_found | 搜索时发现目标 | SEARCH -> APPROACH |
| target_reached | 目标已经足够近 | APPROACH -> STOP |
| target_lost | 靠近过程中目标丢失 | APPROACH -> SEARCH |
其中 area_ratio 是一个简化距离判断方法。
目标越靠近相机，通常检测框越大；因此 area_ratio 越大，可以认为目标越近。
七、task_motion_node 的职责
task_motion_node 负责把任务状态转换成速度命令。
|---------------|------------|
| 输入 | 输出 |
| /task/state | /cmd_vel |
它不关心视觉细节，也不关心事件来源，只关心当前状态。
当前运动规则：
|------------|----------|
| FSM 状态 | 运动行为 |
| IDLE | 停止 |
| STAND | 停止 |
| SEARCH | 原地旋转 |
| APPROACH | 低速前进 |
| STOP | 停止 |
| REPORT | 停止 |
| ERROR | 停止 |
这体现了一个重要工程原则：决策和控制分离。
FSM 只决定"现在是什么状态"，motion 节点负责"这个状态下发什么速度"。
尤其要注意：ERROR 状态下必须输出零速度，因为异常时默认行为应该是停止，而不是继续运动。
八、robot_control_node、driver 与 heartbeat
robot_control_node 是 ROS2 速度命令和底层机器人 driver 之间的桥。
|------------|----------------------|-----------------|
| 上层输入 | 中间节点 | 底层执行 |
| /cmd_vel | robot_control_node | MockDogDriver |
这样上层控制逻辑不需要知道具体机器人 SDK 的细节。
上层只发布标准 ROS2 速度命令，底层 driver 负责适配具体机器人。
heartbeat 是控制安全机制。
|------------------|---------------------------|
| heartbeat 状态 | 控制行为 |
| 正常 | 执行 /cmd_vel |
| 超时 | 忽略 /cmd_vel，调用 stop() |
| 未启动 | 认为不安全，不执行速度 |
heartbeat 的意义是：
防止上层节点异常退出后，机器人继续执行旧速度命令。
九、安全保护节点
task_safety_node 是独立安全监控节点。
它不直接发布 /cmd_vel，而是把异常转换成 FSM 事件。
|----------------|-------------|
| 订阅 | 作用 |
| /robot/state | 判断机器人本体是否安全 |
| /task/state | 判断任务是否卡住或超时 |
|---------------|---------|
| 发布 | 内容 |
| /task/event | error |
安全处理链路：
|--------|------------|------------|----------|
| 异常 | 安全节点输出 | FSM 状态 | 运动结果 |
| 低电量 | error | ERROR | 停止 |
| 急停 | error | ERROR | 停止 |
| 任务超时 | error | ERROR | 停止 |
这个设计的好处是：
安全节点不抢控制权，而是让 FSM 进入 ERROR ，再由 motion 节点统一输出零速度。
这样系统不会出现多个节点同时发布 /cmd_vel 的控制冲突。
十、完整任务系统的数据流
主任务链路：
/perception/target → vision_fsm_bridge_node → /task/event → task_fsm_node → /task/state → task_motion_node → /cmd_vel → robot_control_node → MockDogDriver
安全保护链路：
/robot/state + /task/state → task_safety_node → /task/event:error → task_fsm_node → ERROR → task_motion_node → /cmd_vel=0
对应工程分层：
|--------|----------------------------------------|
| 层级 | 项目模块 |
| 感知层 | AprilTag / YOLO / /perception/target |
| 事件桥接层 | vision_fsm_bridge_node |
| 决策层 | task_fsm_node |
| 控制层 | task_motion_node |
| 执行层 | robot_control_node + MockDogDriver |
| 安全层 | heartbeat + task_safety_node |