ROS2 任务状态机、视觉桥接与安全控制闭环复盘：从 /perception/target 到 /cmd_vel 的机器人目标搜索与靠近实现

一、FSM 的基本概念

FSM 是 Finite State Machine，有限状态机。

在机器人任务中，FSM 用来描述机器人处于哪个任务阶段，以及收到事件后应该切换到哪个状态。

核心公式可以理解为：

当前状态 + 输入事件 = 下一个状态

例如机器人执行“寻找目标并靠近”的任务时，不应该一直发布同一个速度，而是要经历：

空闲 → 准备 → 搜索 → 靠近 → 停止

FSM 的价值是把复杂任务拆成清晰状态，避免控制逻辑写成一堆混乱的 if-else。

二、状态和事件的区别

FSM 中最重要的是区分 状态 state 和 事件 event。

状态表示“现在是什么”。 事件表示“发生了什么”。

在我的项目里：

所以 /task/event 和 /task/state 不能混用。前者是触发条件，后者是任务结果。

三、FSM 状态设计

我项目中设计了 7 个状态。

主状态转移关系：

异常转移关系：

注意：stand_done 只在 STAND 状态下有效。如果机器人还在 IDLE，直接发送 stand_done 被忽略是正常现象。

四、task_fsm_node 的职责

task_fsm_node 是状态机节点。

它只负责一件事：

接收任务事件，更新任务状态，发布当前状态。

它不直接发布 /cmd_vel。

这样设计的核心思想是：

FSM 负责“任务决策”，不负责“底层运动”。

也就是说：

这种分层让后续升级更容易。比如以后把 APPROACH 从固定前进改成视觉跟随，不需要重写 FSM。

五、视觉桥接节点的作用

视觉节点输出的是感知结果，例如是否检测到目标、置信度、目标框大小、目标位置等。

但 FSM 不应该直接处理复杂视觉字段。

所以需要 vision_fsm_bridge_node，它的作用是：

把视觉检测结果转换成 FSM 能理解的任务事件。

核心逻辑：

这样视觉层不会直接控制机器人，而是通过事件影响 FSM，再由 FSM 影响运动。

六、目标检测字段与任务事件

/perception/target 中常见字段如下。

三个核心事件：

其中 area_ratio 是一个简化距离判断方法。

目标越靠近相机，通常检测框越大；因此 area_ratio 越大，可以认为目标越近。

七、task_motion_node 的职责

task_motion_node 负责把任务状态转换成速度命令。

它不关心视觉细节，也不关心事件来源，只关心当前状态。

当前运动规则：

这体现了一个重要工程原则：决策和控制分离。

FSM 只决定“现在是什么状态”，motion 节点负责“这个状态下发什么速度”。

尤其要注意：ERROR 状态下必须输出零速度，因为异常时默认行为应该是停止，而不是继续运动。

八、robot_control_node、driver 与 heartbeat

robot_control_node 是 ROS2 速度命令和底层机器人 driver 之间的桥。

这样上层控制逻辑不需要知道具体机器人 SDK 的细节。

上层只发布标准 ROS2 速度命令，底层 driver 负责适配具体机器人。

heartbeat 是控制安全机制。

heartbeat 的意义是：

防止上层节点异常退出后，机器人继续执行旧速度命令。

九、安全保护节点

task_safety_node 是独立安全监控节点。

它不直接发布 /cmd_vel，而是把异常转换成 FSM 事件。

安全处理链路：

这个设计的好处是：

安全节点不抢控制权，而是让 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

对应工程分层：