Hands On High Ceiling

手把手 5：把示例升级到工程上限

本课把前四课组合起来，构建一个接近工程上限的完整示例。

目标：

SinWaveState
  -> 支持键盘、手柄、UDP driver 三种入口
  -> 支持安全确认
  -> 使用自定义过渡
  -> 支持 pause action
  -> 有自动返回
  -> 状态图可视化
  -> 发布时可保护删除

1. 最终状态机事件规划

我们规划三个 event：

sin_wave
toggle_sin_pause
normal

remote_events：

remote_events:
  sin_wave:
    slot: btn_10
    value: 5
  toggle_sin_pause:
    slot: btn_9
    value: 1
  normal:
    slot: btn_1
    value: 1

2. 最终遥控器 command 规划

我们规划两个 command：

command.sin_wave
command.toggle_sin_pause

输入来源：

• 键盘：z + 0 进入 sin_wave。
• 手柄：trigger.left + trigger.right + button.west 进入。
• UDP：udp.mode=high + udp.button.a 进入。
• pause：键盘 p 或手柄 button.west 单独按。

3. sources 汇总

键盘：

sources:
  keyboard:
    type: keyboard
    signals:
      keyboard.arm: {from: keyboard.key, key: "z"}
      keyboard.sin_wave: {from: keyboard.key, key: "0"}
      keyboard.pause: {from: keyboard.key, key: "p"}

UDP：

sources:
  udp:
    type: udp
    bind: 0.0.0.0
    port: 14550
    signals:
      udp.vx: {from: udp.vx, timeout_ms: 500, failsafe: 0.0}
      udp.vy: {from: udp.vy, timeout_ms: 500, failsafe: 0.0}
      udp.yaw: {from: udp.yaw, timeout_ms: 500, failsafe: 0.0}
      udp.a: {from: udp.a, timeout_ms: 500, failsafe: 0.0}
      udp.mode: {from: udp.mode, timeout_ms: 500, failsafe: 0.0}

4. controls 汇总

controls:
  keyboard.arm: {type: bool, source: keyboard.arm}
  keyboard.sin_wave: {type: bool, source: keyboard.sin_wave}
  keyboard.pause: {type: bool, source: keyboard.pause}

  udp.button.a:
    type: bool
    source: udp.a
    threshold: 0.5

  udp.mode:
    type: enum
    source: udp.mode
    default: middle
    hysteresis: 0.03
    positions:
      low: [-1.0, -0.35]
      middle: [-0.34, 0.34]
      high: [0.35, 1.0]

  command.sin_wave:
    type: bool
    expr:
      any:
        - [keyboard.arm, keyboard.sin_wave]
        - [trigger.left, trigger.right, button.west]
        - [udp.mode=high, udp.button.a]

  command.toggle_sin_pause:
    type: bool
    expr:
      any:
        - [keyboard.pause]
        - all:
            - button.west
            - released: shoulder.left
            - released: shoulder.right

这个例子教会你：简单组合用短数组，复杂条件用显式 all。released 这种 map 条件不要硬塞进一行短数组里，显式展开更稳。

5. outputs 汇总

outputs:
  conflict_policy: first_wins
  publish_on_change: true

  analog:
    vel_des.x:
      controls: [move.vx, udp.move.vx]
      mix: max_abs
    vel_des.y:
      controls: [move.vy, udp.move.vy]
      mix: max_abs
    yawdot_des:
      controls: [move.yaw, udp.move.yaw]
      mix: max_abs

  edge:
    - output: btn_10=5
      when: [command.sin_wave]
    - output: btn_9=1
      when: [command.toggle_sin_pause]

状态切换和 pause 都用 edge，避免每帧重复触发。

6. 自定义过渡 profile

来自第 3 课：

transition_profiles:
  pose_blend_switch:
    exit_duration: 0.02
    enter_duration: 0.4
    exit_behavior: hold_last_motor
    enter_behavior: first_frame_blend_pose_gain
    data:
      qpos_start: current
      kp_start: zero
      kd_start: target
      curve: smoothstep

7. 状态配置

states:
  normal:
    behavior: NormalState
    speed_profile: normal
    transitions:
      on_event:
        sin_wave:
          to: sin_wave
          transition: pose_blend_switch

  sin_wave:
    behavior: SinWaveState
    params:
      joint: 22
      amplitude: 0.4
      frequency: 1.0
      duration: 0.0
    transitions:
      on_event:
        normal:
          to: normal
          transition:
            base: pose_blend_switch
            enter_duration: 0.2
        zero_torque: zero_torque
        toggle_sin_pause:
          action: toggle_sin_pause
      after:
        - seconds: 6.0
          to: normal
          transition: soft_switch

这里同时展示：

• 事件切状态。
• action 不切状态。
• after 自动返回。
• inline transition 覆盖。

8. 状态图导出

graph:
  validate: true
  export:
    dot: /tmp/elf3_state_machine.dot
    mermaid: /tmp/elf3_state_machine.mmd

启动后查看：

cat /tmp/elf3_state_machine.mmd

你应该看到类似：

stateDiagram-v2
    [*] --> zero_torque
    normal --> sin_wave: sin_wave [pose_blend_switch]
    sin_wave --> normal: normal [inline...]

9. 调试状态机信息

ros2 topic echo /simulation/state_machine_info

重点看：

• events 是否出现 sin_wave。
• current.name 是否进入 sin_wave。
• transition.enter_behavior 是否是 first_frame_blend_pose_gain。
• transition.data 是否是你配置的值。
• pending 是否为空或符合 delay。

10. 发布保护

如果 sin_wave 也属于内部状态，可以加入：

protected_states:
  sin_wave:
    behavior: SinWaveState
    events: [sin_wave]
    model_keys: []
    files: []

如果状态已经拆到独立文件，未来可以增强脚本直接删文件。当前清单仍然能表达：

• 删除状态。
• 删除 event。
• 删除引用 event 的遥控器 binding。
• 删除 behavior class。

11. 这个例子展示了哪些上限

输入层上限：

• 键盘、手柄、UDP driver 混合。
• 轴、按钮、enum 三档开关统一。
• 复杂组合键用 command.* 管理。

输出层上限：

• analog 支持多个 control 混合。
• edge 支持一次性事件。
• level 可继续兼容旧持续按钮。

状态机上限：

• event 切换。
• action。
• after 自动转移。
• inline transition。
• graph 自检和导出。

过渡层上限：

• 通用 enter_behavior。
• transition.data 私有参数。
• 状态第一帧。
• 姿态和 gain 同时渐变。

发布层上限：

• 高危状态、event、模型、文件发布时删除。
• 非保护引用保留并 warning。

12. 工程化 checklist

1. 每个物理输入都先变成 source。
2. 业务组合只写 controls。
3. outputs 不直接写复杂物理按键逻辑。
4. 状态切换用 remote_events。
5. 状态类只关心当前状态运行逻辑。
6. 状态私有变量放 self。
7. 过渡共用逻辑放 robot_state_base.py。
8. 过渡私有参数放 transition.data。
9. driver 不写业务状态名。
10. 发布保护只通过 release_protection.yaml 描述。