多策略运行时与热切换机制

本文档聚焦推理节点内部的多策略并行架构与运行时热切换机制，阐述系统如何在单一进程内托管多个异构策略模型，并通过手柄按键或 ROS 服务在不停机的情况下完成策略切换、动作参考切换与外部动作覆盖。

核心架构：PolicyRuntime 与策略池

推理节点 InferenceNode 在构造阶段通过 ROS 参数加载一组策略配置，并为每个策略实例化一个 PolicyRuntime 对象。这些对象以数组 policies_ 的形式常驻内存，构成策略池。

PolicyRuntime 内部封装了策略所需的全部运行时资源，包括独立的 ONNX 推理会话、观测内存、帧堆叠缓冲区、动作输出缓存以及可选的 Motion 参考序列加载器。这种设计使得每个策略在运行时拥有完全隔离的上下文，切换策略时只需改变索引，无需重新加载模型或重建会话。

struct PolicyRuntime {
    std::string name;
    std::string model_path;
    std::string motion_path;
    std::vector<ObsSourceSpec> obs_layout;
    std::vector<int> obs_layout_sizes;
    std::vector<std::vector<float>> obs_segments;
    std::vector<float> obs;
    std::vector<ObsSourceSpec> extra_obs_layout;
    std::vector<std::vector<float>> extra_obs_segments;
    int obs_num = 0;
    int extra_obs_num = 0;
    int frame_stack = 1;
    ObsStackOrder stack_order = ObsStackOrder::FrameMajor;
    std::unique_ptr<ModelContext> ctx;
    std::shared_ptr<MotionLoader> motion_loader;
    size_t motion_frame = 0;
    bool is_first_frame = true;
};

ModelContext 则进一步封装了 ONNX Runtime 的会话、内存信息、输入输出张量及其元数据。系统在初始化阶段会为每一个 PolicyRuntime 调用 setup_model()，一次性完成模型校验、张量分配与优化图构建。

Sources: inference_node.hpp

运行时状态机

策略池之上，推理节点维护一组原子变量和索引来控制运行时行为：

状态变量	类型	语义
`active_policy_idx_`	`int`	当前活跃策略在 `policies_` 中的索引
`motion_policy_indices_`	`vector<int>`	标记所有关联了 motion 文件的策略索引
`is_motion_policy_`	`atomic<bool>`	当前是否处于 Motion 策略模式
`current_motion_policy_idx_`	`size_t`	当前在 `motion_policy_indices_` 中选中的 Motion 策略游标
`is_interrupt_`	`atomic<bool>`	是否启用外部动作中断覆盖

推理线程每次循环时通过 active_policy() 获取当前策略引用，随后所有的观测组装、帧堆叠、模型推理均基于该策略的独立配置进行。

Sources: inference_node.hpp

策略配置与初始化流程

多策略通过 ROS 2 参数列表进行声明式配置。在 load_config() 中，系统读取等长的参数数组 model_names、obs_layouts、frame_stacks、obs_stack_orders，以及可选的 motion_names 和 extra_obs_layouts。每个数组的同一索引位置共同描述一个策略的完整配置。

以 inference_beyondmimic.yaml 为例，该配置同时注册了四个策略：一个基础行走策略和三个基于动作参考的展示策略（wave、dance、punch）。

model_names: ["policy.onnx", "policy_wave.onnx", "policy_dance.onnx", "policy_punch.onnx"]
motion_names: ["", "wave.npz", "dance.npz", "punch.npz"]
obs_layouts:
  - "ang_vel:3, gravity_b:3, cmd_vel:3, dof_pos:23, dof_vel:23, last_action:23"
  - "motion_pos:23, motion_vel:23, ang_vel:3, gravity_b:3, dof_pos:23, dof_vel:23, last_action:23"
frame_stacks: [10, 1, 1, 1]
obs_stack_orders: ["frame_major", "frame_major", "frame_major", "frame_major"]

初始化阶段，系统会对每个策略执行以下操作：

解析观测布局：将字符串形式的布局声明解析为 ObsSourceSpec 数组，计算总观测维度 obs_num。
加载 Motion 参考：若配置了 motion_path，则实例化 MotionLoader 并校验关节数量一致性。
创建 ONNX 会话：根据 model_path 创建 Ort::Session，校验模型输入尺寸与 (obs_num * frame_stack + extra_obs_num) 是否匹配。
分配运行时内存：为 obs、obs_segments、input_buffer、output_buffer 等分配零初始化的向量。

所有策略在节点启动后即全部就绪，运行时切换不涉及任何模型加载或内存分配操作。

Sources: ros_interface.cpp

热切换机制详解

推理节点提供两种热切换语义，分别对应两类典型应用场景：Motion Policy 切换与 Interrupt 动作覆盖。两者共享同一套暂停-切换-重置-恢复的安全协议，但底层实现有所不同。

安全切换协议：switch_while_paused

无论哪种切换模式，系统都不允许在推理循环中途直接修改活跃策略的状态。手柄 LB 键（buttons[4]）触发时，会进入 switch_while_paused 闭包：

获取 lb_switch_mutex_，防止重入。
原子交换 is_running_ 为 false，若推理正在运行则先暂停。
在 mode_mutex_ 保护下执行具体的切换逻辑。
若之前处于运行状态，则自动恢复推理。

该协议确保切换期间推理线程处于空闲休眠状态，避免观测缓冲区、动作输出或策略内部状态在切换边界出现数据竞争。

Sources: ros_interface.cpp

Motion Policy 模式切换

当配置中至少有一个策略关联了 Motion 文件时（has_motion_policy() 返回 true），LB 键用于在基础策略（索引 0，通常是自由行走策略）与当前选中的 Motion 策略之间进行二元切换。

切换逻辑会执行以下原子操作序列：

翻转 is_motion_policy_ 标志。
若进入 Motion 模式，则令 active_policy_idx_ = motion_policy_indices_[current_motion_policy_idx_]。
若退出 Motion 模式，则回到基础策略 active_policy_idx_ = 0。
调用 reset_policy_runtime(active_policy())，将新激活策略的观测、输入缓冲区清零，motion_frame 归零，is_first_frame 置为 true。

RB 键（buttons[5]）则负责在基础策略状态下预选下一个 Motion 策略，通过 current_motion_policy_idx_ = (current_motion_policy_idx_ + 1) % motion_policy_indices_.size() 循环遍历。需要注意，RB 键仅在非 Motion 模式下生效；若在 Motion 模式运行时按下 RB，系统会发出警告并拒绝切换，防止在动作执行中途被意外打断。

Sources: ros_interface.cpp

Interrupt 动作覆盖模式

当配置中包含 interrupt 观测源时（supports_interrupt() 返回 true），系统进入中断覆盖模式。此时 LB 键不再切换策略，而是翻转 is_interrupt_ 原子标志。

Interrupt 机制的工作流程如下：

观测注入：get_interrupt_obs() 根据 is_interrupt_ 的值向模型输入 0 或 1，使策略网络感知到外部覆盖请求的开关状态。
外部动作订阅：当 is_interrupt_ 为 true 时，subs_joint_state_callback 会持续将 /joint_ref_states 话题中的关节位置写入 interrupt_action_ 缓冲区。
动作覆盖：在推理循环的 action 组装阶段，若 is_interrupt_ 为 true，系统会将 interrupt_action_ 中保存的关节位置直接覆盖到 act_ 数组的对应尾部关节上，而非使用模型输出。

这种设计允许外部模块（如遥操作或安全夹持器）在特定关节上接管控制权，而基础策略仍继续运行并为其余关节生成动作。

Sources: ros_interface.cpp, obs_manager.cpp, ros_interface.cpp, inference_node.cpp

推理循环中的策略隔离

推理线程以固定周期运行，其核心逻辑如下：

auto& policy = active_policy();
update_obs_segments(policy.obs_segments, policy.obs_layout);
flatten_obs_segments(policy.obs_segments, policy.obs.begin());
update_stacked_obs(policy.ctx->input_buffer, policy.obs, ...);
if(policy.extra_obs_num > 0) { /* 组装 extra obs */ }
if (policy.motion_loader) { step_motion_frame(); }
policy.ctx->session->Run(...);
/* 将 output_buffer 写入 act_ */

关键点在于：

active_policy() 每次循环都会重新解析索引，确保切换后立即生效。
观测与堆叠完全隔离：每个策略拥有自己的 obs_segments、frame_stack 和 stack_order。即使两个策略的观测维度或堆叠方式不同，也不会相互干扰。
Motion 帧推进隔离：step_motion_frame() 操作的是当前活跃策略的 motion_frame，切换策略后该计数器独立演进。
动作输出共享：所有策略最终都向同一个 act_ 数组写入，通过 usd2urdf_ 映射调整关节顺序，再经 act_alpha_ 低通滤波后由控制线程下发。

Sources: inference_node.cpp

配置模式对比

下表总结了仓库中提供的典型多策略配置模式，帮助开发者根据场景选择合适的热切换语义：

配置文件	策略数量	Motion 策略	Interrupt 源	热切换语义	典型场景
`inference.yaml`	1	无	无	无切换	单一行走策略
`inference_amp.yaml`	1	无	无	无切换	AMP 风格运动
`inference_attn_enc.yaml`	1	无	无	无切换	带高程感知的感知策略
`inference_getup.yaml`	2	有（getup）	无	LB 切换基础/起身	摔倒后起身恢复
`inference_beyondmimic.yaml`	4	有（wave/dance/punch）	无	LB 切换基础/Motion；RB 预选 Motion	多动作展示与模仿
`inference_interrupt.yaml`	1	无	有	LB 开关中断覆盖	遥操作与外部夹持

Sources: config/inference_beyondmimic.yaml, config/inference_getup.yaml, config/inference_interrupt.yaml

热切换时序图

以下时序图展示了从基础策略通过 LB 键切换到 Motion 策略的完整安全协议：

sequenceDiagram
    participant User as 手柄(LB)
    participant Joy as Joy Callback
    participant Inf as Inference 线程
    participant Policy as PolicyRuntime

    User->>Joy: 按下 LB
    Joy->>Joy: 获取 lb_switch_mutex_
    Joy->>Joy: is_running_.exchange(false)
    Note over Inf: 循环检测到 !is_running，<br/>进入休眠
    Joy->>Joy: 获取 mode_mutex_
    Joy->>Joy: is_motion_policy_ = true
    Joy->>Joy: active_policy_idx_ = motion_idx
    Joy->>Policy: reset_policy_runtime(policy)
    Note over Policy: obs清零, motion_frame=0,<br/>is_first_frame=true
    Joy->>Joy: 释放 mode_mutex_
    Joy->>Joy: is_running_.store(true)
    Note over Inf: 循环恢复运行
    Inf->>Inf: active_policy() 返回新策略
    Inf->>Policy: 组装 obs, 推理, 推进 motion_frame

策略状态重置的边界语义

reset_policy_runtime() 是热切换安全性的核心保障。切换策略时，新策略的内部状态必须与旧策略彻底解耦，否则会导致：

观测堆叠污染：旧策略的历史帧残留在输入缓冲区中，使新策略的首帧推理基于错误的时序上下文。
Motion 帧错位：新策略若使用 Motion 参考，必须从第 0 帧开始播放，否则动作参考与真实机器人状态出现相位差。
last_action 残留：output_buffer 中的旧动作值若直接作为新策略的 last_action 观测输入，会引入瞬态跳变。

因此，每次 LB 切换后都会执行一次完整的运行时重置，将新策略的所有内部缓存归零，并标记 is_first_frame = true，触发帧堆叠的冷启动填充逻辑（将首帧观测复制到所有历史槽位）。

Sources: inference_node.cpp, inference_node.cpp

与实时线程的协同

多策略运行时与热切换机制建立在实时双线程架构之上。inference() 线程以 dt * decimation 的周期执行策略推理与观测更新，control() 线程以 dt 的周期从共享的 act_ 数组读取动作并下发给电机。

切换操作仅在 inference() 线程侧通过 is_running_ 进行启停控制，control() 线程不受影响，持续以固定频率运行。这种设计保证了即使在策略切换的瞬间，电机控制仍然维持稳定的实时节拍，不会因模型推理或状态重置而出现控制断档。关于双线程调度与优先级设置的详细内容，请参阅实时线程设计文档。

相关页面：实时双线程设计与调度策略

后续阅读建议

若需了解观测布局字符串的解析规则与动态注册机制，请参阅观测布局配置与动态解析。
若需了解 Motion 文件的加载格式与 motion_pos/motion_vel 观测源的数学含义，请参阅动作参考加载与 MotionLoader。
若需了解 Interrupt 模式与外部关节覆盖的完整 ROS 接口约定，请参阅中断动作与外部覆盖机制。
若需了解 ONNX Runtime 会话的创建细节与优化选项，请参阅 ONNX Runtime 推理引擎集成。