一、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