推理节点设计与线程模型

推理节点（inference_node）是整个机器人控制系统的实时决策中枢，它负责以固定频率执行强化学习策略的 ONNX 推理，并将生成的动作指令安全地同步到低层电机控制回路。本文档从线程分层、数据同步、实时调度和多策略运行时四个维度，系统性地剖析其架构设计原理与实现细节。理解这些机制是排查推理延迟、控制抖动及策略切换异常的前提。

Sources: inference_node.hpp, inference_node.cpp

推理节点整体架构

InferenceNode 继承自 rclcpp::Node，但在 ROS2 标准回调模型的基础上引入了两条独立的实时线程：inference_thread_ 负责周期性神经网络推理，control_thread_ 负责周期性动作下发与电机通信。ROS2 的 MultiThreadedExecutor 仅用于处理外部订阅、服务和话题发布，不介入实时控制路径。这种分离确保了通信层的事件处理不会阻塞控制回路的确定性时序。

节点内部通过 RobotInterface 聚合硬件访问能力，并借助 ThreadPool 在多路 CAN/CANFD 总线上并行执行电机指令。每条策略的运行时状态被封装为独立的 PolicyRuntime 实例，使多策略热切换无需重新初始化 ONNX Session。

graph TB
    subgraph ROS2通信层["ROS2 通信层 (Executor 2 threads, SCHED_FIFO 50)"]
        A1[Joy订阅]
        A2[cmd_vel订阅]
        A3[感知订阅]
        A4[服务接口]
        A5[状态发布]
    end

    subgraph 推理节点["InferenceNode 核心"]
        B1[inference_thread<br/>SCHED_FIFO 70]
        B2[control_thread<br/>SCHED_FIFO 70]
        B3[PolicyRuntime 0..N]
        B4[act_ / last_act_ 双缓冲]
    end

    subgraph 硬件抽象层["RobotInterface 硬件抽象层"]
        C1[ThreadPool<br/>SCHED_FIFO 70]
        C2[IMU驱动]
        C3[电机驱动总线0]
        C4[电机驱动总线N]
    end

    A1 -->|cmd_vel_| B1
    A3 -->|perception_obs_| B1
    B1 -->|act_| B4
    B2 -->|last_act_| C1
    B4 --> B2
    C1 --> C3
    C1 --> C4
    B1 --> A5

Sources: inference_node.hpp, inference_node.cpp

线程模型详解

推理节点采用了三层线程架构：ROS2 Executor 线程、推理线程、控制线程。各线程的职责、调度策略和周期严格分离，形成清晰的实时层级。

线程/线程池	职责	调度策略	典型周期	优先级
`main` + Executor	节点初始化、参数加载、ROS 回调处理	`SCHED_FIFO`	事件驱动	50
`inference`	观测采集、堆叠、ONNX 推理、动作生成	`SCHED_FIFO`	`dt × decimation`	70
`control`	动作指数平滑、调用 `RobotInterface::apply_action`	`SCHED_FIFO`	`dt`	70
`ThreadPool`	并行执行多总线电机 MIT 指令	`SCHED_FIFO`	随 control 触发	70

main 函数在初始化时通过 mlockall(MCL_CURRENT | MCL_FUTURE) 将进程内存锁定，防止运行时发生页交换导致的非确定性延迟。若任何实时线程未能成功设置 SCHED_FIFO，节点会立即调用 rclcpp::shutdown() 终止运行，避免在降级调度下产生不可控的控制行为。

Sources: inference_node.cpp, thread_pool.hpp

推理线程与控制线程的时序协作

inference 线程与 control 线程是节点内最关键的实时闭环。两者的协作遵循生产者-消费者模式，并通过 act_mutex_ 保护的共享缓冲区 act_ 进行握手。control 线程以 dt 为周期高频运行（如 4 ms，250 Hz），而 inference 线程以 dt × decimation 为周期低频运行（如 20 ms，50 Hz）。这意味着在相邻两次推理之间，control 线程会执行 decimation 次动作下发。

sequenceDiagram
    participant I as inference_thread
    participant B as act_ / last_act_ 缓冲区
    participant C as control_thread
    participant R as RobotInterface

    loop 每 dt × decimation 周期
        I->>I: 采集观测 / 堆叠历史帧
        I->>I: ONNX Session::Run
        I->>B: 写入 act_ (act_mutex_)
    end

    loop 每 dt 周期
        C->>B: 读取 act_ (act_mutex_)
        C->>C: last_act_ = α·act_ + (1-α)·last_act_
        C->>R: apply_action(last_act_)
        R->>R: 多总线并行电机指令
    end

apply_action() 在 control 线程中被调用时，执行的是动作指数平滑（exponential smoothing）：last_act_ = act_alpha_ * act_ + (1 - act_alpha_) * last_act_。当 act_alpha_ = 1.0 时，last_act_ 直接跟踪 act_；当 act_alpha_ < 1.0 时，控制输出会在两次推理间隔内呈现平滑过渡，降低因策略离散跳变带来的机械冲击。

Sources: inference_node.cpp, inference_node.cpp

数据流与同步机制

由于多条线程并发访问共享资源，节点内部定义了六把细粒度互斥锁，分别保护不同生命周期和访问模式的状态。

互斥锁	保护数据	访问线程	说明
`act_mutex_`	`act_`, `last_act_`	inference, control	推理输出与平滑输入的临界区
`perception_mutex_`	`perception_obs_buffer_`	inference, ROS订阅回调	外部感知数据（如地形高程）
`interrupt_mutex_`	`interrupt_action_`	inference, ROS订阅回调	中断动作（如手柄直接控关节）
`cmd_mutex_`	`cmd_vel_`	inference, ROS订阅回调	线速度/角速度指令
`mode_mutex_`	`active_policy_idx_`, policy 运行时状态	inference, ROS服务/订阅	策略切换与 motion 状态
`lb_switch_mutex_`	LB 按钮切换的暂停/恢复逻辑	ROS订阅回调	防止切换期间竞态

inference 线程在每次循环开始时先加锁 mode_mutex_ 获取当前激活策略的引用，随后采集观测、执行推理，最后加锁 act_mutex_ 写入新生成的动作。control 线程则在每次循环中加锁 act_mutex_ 读取最新动作并更新平滑值。这种锁分离策略确保了高频 control 线程不会被低频 inference 线程长时间阻塞——act_mutex_ 的临界区仅涉及向量拷贝与简单算术运算，持锁时间极短。

Sources: inference_node.hpp, inference_node.cpp

多策略运行时设计

为支持行走、舞蹈、起身等多策略热切换，InferenceNode 在初始化时为配置文件中声明的每个 ONNX 模型创建独立的 PolicyRuntime 实例。每个实例持有完整的推理上下文，包括 ModelContext（ONNX Session、输入输出 Tensor、内存信息）、观测布局 obs_layout、运动加载器 motion_loader 及帧历史状态。

classDiagram
    class InferenceNode {
        +vector~PolicyRuntime~ policies_
        +atomic~bool~ is_running_
        +atomic~bool~ is_interrupt_
        +int active_policy_idx_
        +thread inference_thread_
        +thread control_thread_
        +inference()
        +control()
    }
    class PolicyRuntime {
        +string name
        +string model_path
        +vector~ObsSourceSpec~ obs_layout
        +vector~float~ obs
        +int frame_stack
        +ObsStackOrder stack_order
        +ModelContext ctx
        +shared_ptr~MotionLoader~ motion_loader
        +bool is_first_frame
    }
    class ModelContext {
        +unique_ptr~Ort::Session~ session
        +unique_ptr~Ort::Value~ input_tensor
        +unique_ptr~Ort::Value~ output_tensor
        +vector~float~ input_buffer
        +vector~float~ output_buffer
    }
    InferenceNode --> PolicyRuntime
    PolicyRuntime --> ModelContext

策略切换通过手柄 LB 键或 ROS 服务触发。切换发生时，inference 线程会先被暂停（is_running_ 置 false），随后 active_policy_idx_ 被修改，目标策略的 is_first_frame 被重置为 true，观测堆叠缓冲区会被初始填充，最后恢复运行。该过程由 mode_mutex_ 和 lb_switch_mutex_ 双重保护，确保切换瞬间不会发生观测片段跨策略混用。

Sources: inference_node.hpp, ros_interface.cpp

实时保障与故障处理

推理节点的实时性不仅依赖线程优先级，还通过以下机制得到强化：

1. ONNX Runtime 线程控制 setup_model() 在创建 Session 时显式调用 session_options.DisablePerSessionThreads()，避免 ONNX Runtime 内部创建额外的线程池与实时线程争夺 CPU。若需要利用多核进行算子内并行，可通过配置参数 intra_threads 控制 Ort::ThreadingOptions 的全局线程数，但默认设置为 1，以最小化调度不确定性。

2. 推理超时告警 inference 线程在每次循环结束时精确计算执行耗时。若单次循环耗时超过设定周期，节点会输出 Inference loop overran 警告，提示开发者当前模型或观测预处理存在性能瓶颈，可能需要降低模型复杂度或提升硬件算力。

3. 安全监控与快速停机 节点内置了两级硬性安全检查：一是 get_gravity_b_obs() 监测机体重力向量 Z 分量，当检测到机器人摔倒时立即触发 rclcpp::shutdown()；二是 get_dof_pos_obs() 监测各关节是否超出机械限位，超限同样立即停机。这些检查运行在 inference 线程内部，因此安全响应延迟受限于推理周期（通常为 20 ms 量级）。

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

后续阅读建议

掌握线程模型后，建议继续深入以下相关主题以形成完整的推理系统认知：

若需理解 ONNX Session 的创建细节、Tensor 内存布局及 CPU 执行提供者的优化选项，请参阅 ONNX 模型加载与实时推理。
若需了解观测堆叠的 frame_major 与 obs_major 两种时序排列方式，以及多策略切换的完整配置范式，请参阅观测堆叠与多策略切换。
若需深入 RobotInterface 内部的电机总线并行通信、IMU 坐标变换和闭链解耦计算，请参阅 RobotInterface 硬件抽象层。
若需调整控制频率、平滑系数或配置新的感知观测话题，请参阅配置文件系统详解。