这篇文章只讲一件事，怎么在启元 Q1 上从零写一个摸头问候 Skill。

需求很具体。用户摸一下机器人的头，机器人先说一句你好，再挥一次手，表情和灯带同步亮一下，做完回到原来的运动状态。摸头不能连续触发，走路时不能触发，电量低不能触发，有动画在播时不能触发。

看起来简单。真写起来，难点不在调接口，在什么时候不调接口。

我把整个过程拆成十步，接口选择、状态机、代码、单元测试、mock 演示、ROS2 联调、打包、上传前检查。每一步都有真实代码和真实日志，文中引用的官方文档章节都标了出处。

一、需求拆成可验收的行为

先把需求写成可验收的条目。

编号	行为	验收标准
R1	单次摸头触发	一次 event_type 为 1 的触摸，进入完整周期
R2	防抖	300 毫秒内的重复触摸只算一次
R3	冷却	一次完整周期后 3 秒内不再触发
R4	低电量拒绝	电量低于 20% 直接拒绝
R5	走路拒绝	当前运动状态是 WALK 时拒绝
R6	动画占用拒绝	有动画在播且不可打断时拒绝
R7	先说话再挥手	TTS 调用成功后再发预设动作
R8	降级	TTS 失败不挡挥手，预设动作被拒不挡表情和灯带
R9	状态恢复	周期结束后切回进入前的运动状态
R10	可打包上传	ROS2 包结构完整，配置外置，README 齐全

这十条就是后面所有代码和测试的依据。没有验收标准的需求，写完也不知道对不对。

二、接口从官方文档里选

摸头问候需要六个接口。

触摸输入，/aima/hal/touch/state，官方文档定义的话题，消息类型 TouchState，event_type 为 1 表示单次点击。

语音播报，/aimdk_5Fmsgs/srv/PlayTts，服务，消息类型 PlayTts。请求里带文本、优先级、domain、是否允许打断。

动作切换，/aimdk_5Fmsgs/srv/SetMcAction，服务，消息类型 SetMcAction。用来切到 BIPED_WHOLE_BODY_CTRL，也用来恢复进入前的状态。

预设动作，/aimdk_5Fmsgs/srv/SetMcPresetMotion，服务，消息类型 SetMcPresetMotion。Q1 的预设动作列表里，3004 是挥手，3005 是开心，3018 是拥抱。

灯带和表情，/aimdk_5Fmsgs/srv/LedStripCommand 和 /aimdk_5Fmsgs/srv/PlayEmotion，服务，用来做反馈。

为什么选预设动作，不直接写关节轨迹。官方接口文档 2.1.1.2 节写了两条硬约束。

预设动作只能在 BIPED_WHOLE_BODY_CTRL 里播。调用之前必须先用 SetMcAction 切过去，再轮询 GetMcAction，确认状态到 RUNNING。

服务端不设置稳态检查，在任何极端运动姿态中发出的动画预设指令都会被立即响应下发。

这两条决定了 Skill 必须先做状态判断，再做动作。预设动作是安全的，前提是你在正确的状态下发它。直接写 22 个关节的轨迹，自由度更大，风险也更大。第一次做 Skill，选预设动作这条笨路。

三、状态机先学会拒绝

状态机有七个状态。

IDLE 空闲。PRECHECK 预检。SPEAK 说话。WAVE 挥手。FEEDBACK 反馈。DONE 收尾。出错进 ERROR。

主流程是 IDLE → PRECHECK → SPEAK → WAVE → FEEDBACK → DONE → IDLE。

拒绝发生在 IDLE 里。触摸事件进来，先过七道判断。

顺序	判断	拒绝理由
1	event_type 不是 1	not_single_tap
2	距上次触摸不到 300 毫秒	debounce
3	状态不是 IDLE	state_busy
4	距上次接受不到 3 秒	cooldown
5	电量低于 20%	battery_low
6	有动画在播	animation_busy
7	当前动作是 WALK	walking

顺序有讲究。防抖放在最前面，因为抖动是最频繁的事件，先过滤掉最省事。状态检查放在冷却前面，因为正在执行中的触摸不是冷却问题，是状态问题。电量和动作检查放在最后，因为这两项要读外部状态，代价高。

这七道判断全部是纯逻辑，不碰 ROS2。这就是为什么状态机可以单独测试。

四、核心代码

状态机是纯 Python，不依赖 ROS2。核心方法 on_touch 长这样。

def on_touch(self, event_type: int, world: WorldState) -> Decision:
    if event_type != 1:
        return self._reject("not_single_tap")
    if world.now - self.last_touch_ts < self.config.debounce_ms / 1000.0:
        return self._reject("debounce")
    self.last_touch_ts = world.now
    if self.state != State.IDLE:
        return self._reject("state_busy")
    if world.now < self.cooldown_until:
        return self._reject("cooldown")
    if world.battery_pct < self.config.min_battery_pct:
        return self._reject("battery_low")
    if world.animation_busy:
        return self._reject("animation_busy")
    if world.current_action in ("WALK", "BIPED_WALK_RUN"):
        return self._reject("walking")
    self.previous_action = world.current_action
    self.state = State.PRECHECK
    return Decision(True, "accepted", [{
        "kind": "ensure_action",
        "target": "BIPED_WHOLE_BODY_CTRL",
        "previous": world.current_action,
    }])

这段代码有三个设计点。

返回值不是 bool，是 Decision。除了接受与否，还带拒绝理由和后续动作步骤。拒绝理由要写进日志，不然排查时只知道没反应，不知道为什么。

接受时记录 previous_action。周期结束后要恢复，不恢复机器人会一直停在 WBC，下一次走跑会怪。

接受时返回一个 ensure_action 步骤，把切模式这件事交给执行层。状态机只做判断，不做 I/O。这样状态机可以脱离 ROS2 测试。

ROS2 节点这一侧，订阅回调只做一件事，把事件标记为 pending。

def on_touch(self, msg: TouchState):
    if msg.event_type == 1:
        self.pending = True
        self.get_logger().info('touch event queued')

真正的判断和执行放在 0.1 秒的定时器里。

def tick(self):
    if not self.pending or self.running:
        return
    self.pending = False
    self.running = True
    ...
    decision = self.runner.handle_touch(1, world)

为什么不在回调里直接执行。回调是执行器在调，你在回调里同步调服务，一卡就把订阅线卡住，后来的触摸事件全丢。事件队列把收和做分开，收永远快，做慢一点没关系。

这是 ROS2 节点的第一条工程经验。订阅回调里不要做重活。

五、配置外置，阈值不写死

所有阈值放进 config/skill.yaml。

debounce_ms: 300
cooldown_s: 3.0
min_battery_pct: 20.0
preset_motion_id: 3004
tts_text: "你好，我在"
tts_priority: 6
tts_domain: "touch_greeting"
feedback_emotion: "happy"
led_color: "warm"
step_timeout_s: 1.0

写死在代码里的阈值，改一次要重编译一次。抽进配置，联调现场改一个数，重启即生效。真机上你会感谢这个设计，因为 300 毫秒的防抖、20% 的电量线，到了真机大概率都要动。

字段命名带单位。debounce_ms 还是 debounce，三个月后你自己都会猜。名字带单位，配置自带文档。

六、十个单元测试

状态机不碰 ROS2，所以可以写纯逻辑测试。我写了 10 个。

测试	验证什么
单次点击接受	IDLE 进 PRECHECK，走完整周期
防抖拒绝	300 毫秒内第二个事件进不来
冷却拒绝	3 秒内第二个事件进不来
非单次触摸拒绝	event_type 过滤生效
低电量拒绝	20% 阈值拦下
走路拒绝	WALK 状态拦下
动画占用拒绝	busy 标志拦下
TTS 失败不挡挥手	SPEAK 失败，继续 WAVE
预设拒接不挡表情	code 1 跳过挥手，表情灯带照常
恢复失败记日志	restore 失败，动作保持 WBC

10 个全过，跑完不到一秒。

图 1，Skill 状态机的 10 个单元测试

单元测试的价值在便宜。逻辑层的 bug 在这里抓，比在联调日志里翻便宜十倍。状态机的分支多，每个拒绝理由都要有测试，不然上线后你会发现某个分支从来没走过。

七、mock 演示，先不碰 ROS2

单元测试过了，下一步是 mock 演示。

mock 后端实现同一套接口，服务调用变成内存里的函数。剧本按时间轴发 10 个触摸事件，覆盖六种拒绝理由和四个接受周期。

真实跑出来的结果。

[  0.00s] touch=1 ACCEPT reason=accepted battery=82
[  0.20s] touch=1 REJECT reason=debounce battery=82
[  0.50s] touch=1 REJECT reason=cooldown battery=82
[  4.00s] touch=1 ACCEPT reason=accepted battery=76
[  8.00s] touch=1 REJECT reason=battery_low battery=12
[ 12.00s] touch=1 REJECT reason=walking battery=70
[ 16.00s] touch=1 REJECT reason=animation_busy battery=70
[ 20.00s] touch=1 ACCEPT reason=accepted battery=70
[ 26.00s] touch=1 ACCEPT reason=accepted battery=70
[ 30.00s] touch=2 REJECT reason=not_single_tap battery=70
events=10 accepted=4 rejected=6 trace_rows=56

图 2，mock 演示的 10 个触摸事件

10 个事件，4 个接受，6 个拒绝。防抖、冷却、低电量、走路、动画占用、非单次触摸，六种拒绝全部出现过。

这份日志比任何架构图都诚实。它证明状态机的每个分支都真的能走到。

八、ROS2 联调，用假机器人对齐契约

mock 证明逻辑对，不能证明 ROS2 链路对。下一步是联调。

我写了一个假机器人节点，6 个服务，2 个话题，消息类型全部对齐 aimdk_msgs 的定义。服务名和官方接口文档一字不差。

• /aimdk_5Fmsgs/srv/GetMcAction
• /aimdk_5Fmsgs/srv/SetMcAction
• /aimdk_5Fmsgs/srv/SetMcPresetMotion
• /aimdk_5Fmsgs/srv/PlayTts
• /aimdk_5Fmsgs/srv/PlayEmotion
• /aimdk_5Fmsgs/srv/LedStripCommand
• /aima/hal/touch/state
• /aima/hal/bms/state

剧本一共十条。第一条电量 82，正常接受。第二第三条连摸，测防抖和冷却。第四条电量 76，再走一遍完整周期。第五条电量掉到 12，吃低电量。第六条切到 WALK，吃走路拒绝。第七条正常。第八条让预设动作返回 code 1，第九条让 TTS 返回失败，专测降级。第十条 touch 等于 2，非单次点击，留给单元测试覆盖。

联调跑完，trace 记下 9 个触摸事件，5 个完整周期，4 个拒绝，一共 66 行。

服务调用统计。

Counter({'GetMcAction': 19, 'SetMcAction': 10, 'PlayTts': 5,
         'SetMcPresetMotion': 5, 'PlayEmotion': 5, 'LedStripCommand': 5})

图 3，ROS2 联调的完整日志

19 次 GetMcAction，对 5 个完整周期，平均一个周期轮询将近 4 次。切入前查一次状态，确认到 RUNNING 查一次，恢复前还要查。轮询是笨办法，可在状态机面前，笨办法最可靠。

10 次 SetMcAction，5 个周期正好每周期 2 次，一次切入 WBC，一次恢复。对得上。

图 4，ROS2 联调里的状态轨迹

轨迹图里每个尖峰是一次完整周期，从 IDLE 爬到 FEEDBACK 再落回来。被拒绝的触摸连状态都没进，只留一根虚线。第 21 秒附近有一根标着 wave_rejected 的线，那是预设动作被服务端拒接的那一次。第 27 秒的 skip 是 TTS 失败后的跳过。

图 5，ROS2 联调里的服务调用次数

九、假服务不是真机，但契约是真的

有人会问，没有真机，联调有什么意义。

意义在契约。服务名、消息类型、调用顺序、状态码语义，这些都是官方文档定义的。假服务对齐了契约，真机换上去，业务代码一行不改。要改的只有服务名前面的命名空间。

联调还暴露了三个 mock 发现不了的问题。

第一个，线程。mock 是单线程顺序执行，ROS2 里订阅回调和定时器在不同线程，状态机被两边同时摸，不加约束就出脏状态。解法是状态访问全部收进定时器，回调只投递事件。

第二个，超时。mock 的服务是本地函数，永不超时。ROS2 的服务走网络，超时要自己设，超时之后的降级要自己写。我给每个服务 2 秒超时，超时按失败处理，直接走降级，不重试。重试在交互场景里经常是错的，人已经转身走了，你还在原地补挥手。

第三个，时序。mock 的时间是脚本给的，ROS2 的时间是真实的。防抖和冷却在真实时钟下的表现，只有联调能验证。

图 6，预设动作返回 code 1 与 TTS 失败的真实日志

预设返回 code 1 时，挥手被跳过，表情和灯带继续。TTS 失败时，挥手照常。每一步的失败只影响它自己，不往上传染。这是把降级写进状态机而不是写进 catch 的原因。catch 是兜底，状态机是规则。

十、打包成 ROS2 包

一个能上传的 Skill，目录结构要完整。

touch_greeting_skill/
├── package.xml
├── setup.py
├── setup.cfg
├── config/skill.yaml
├── launch/touch_greeting.launch.py
├── resource/touch_greeting_skill
├── touch_greeting_skill/
│   ├── __init__.py
│   ├── state_machine.py
│   ├── runner.py
│   ├── backend.py
│   ├── ros2_backend.py
│   ├── skill_node.py
│   └── mock_demo.py
├── test/test_state_machine.py
├── tools/fake_robot_node.py
└── README.md

图 7，Skill 包的目录树

package.xml 声明依赖 rclpy 和 aimdk_msgs。setup.py 注册入口 touch_greeting。launch 放启动参数。config 放阈值。README 写三件事，触发条件、依赖接口、失败怎么恢复。

写清楚失败，比写清楚成功更重要。用你包的人，遇到拒绝时如果有地方查，他就不会以为包是坏的。

十一、上传前的检查清单

检查项	标准
编译	colcon build 出现 finished，无报错
单元测试	10 个全过
mock 演示	10 个事件，4 接受，6 拒绝
ROS2 联调	6 个服务、2 个话题全部对齐
降级	TTS 失败、预设拒接、恢复失败都有日志
配置	阈值全部在 skill.yaml，不写死
README	触发、接口、失败恢复三件事齐全
真机	PrimeConsole 单步验证，未做则明确标注

最后一项我留空。PrimeConsole 的真机单步验证需要真机，我没有，就不写。能在沙箱里验证的，我已经验证到联调层。

十二、真机到手后的第一周

第一天，只做单步。PrimeConsole 里把 6 个服务各手动触发一次，记录返回码和延迟。不看代码，不跑 Skill。

第二天，dry_run。Skill 跑起来，决策写日志，身体不动。摸一天的头，看日志里的接受和拒绝，跟预期对不对。错的部分，先调配置，不调代码。

第三天，放 dry_run。真摸，真挥手。这一天的任务不是加功能，是感受节奏。300 毫秒防抖在真人手上是快是慢，3 秒冷却够不够。

第四天，边界测试。低电量、运动中、动画中、连摸、长按，每一种拒绝路径都人工造一遍。

第五天，打包上传。README 补上真机实测部分。

这个排法的核心是，先验证再调优，先配置再代码。多数人反过来，第三天就开始加功能，第五天还在修第一天的坑。

十三、五条经验

第一，订阅回调只投递事件，不做重活。

第二，状态机不碰 I/O，只做判断。这样它能被单元测试覆盖。

第三，所有阈值外置。真机调参不重编译。

第四，服务调用带超时，超时按失败走降级，不重试。

第五，拒接看 header.code，不看 state。官方文档 2.1.1.2 节写了，state 在拒接时仍是 RUNNING。

这五条不是理论，是联调时一条一条踩出来的。