roboto_inference 是 Atom01 机器人平台的实时策略推理节点，负责将深度强化学习训练得到的 ONNX 策略模型部署到真实硬件。它作为 ROS 2 节点运行，核心设计目标是在严格的实时约束下完成观测组装、帧堆叠、模型推理和动作下发，同时支持多策略热切换、动作参考跟踪与外部中断覆盖等高级运行时能力。整个系统采用 C++17 编写，基于 ONNX Runtime 进行模型推理，并通过分层硬件抽象屏蔽底层电机总线与 IMU 的差异。

Sources: package.xml inference_node.hpp

架构全景

系统由四个逻辑层构成：ROS 2 通信层、实时推理与控制层、硬件抽象层、模型与动作层。通信层负责接收手柄指令、速度指令和外部感知数据，并向外部发布机器人状态；推理与控制层包含两条独立的实时线程，分别执行策略推理和电机指令下发；硬件抽象层通过 RobotInterface 统一管理 IMU 数据读取与多总线电机驱动；模型与动作层则维护若干 ONNX 推理会话和可选的参考动作序列。

graph TB
    subgraph ROS2[ROS 2 通信层]
        JOY["/joy"]
        CMD["/cmd_vel"]
        ELEV["/elevation_data"]
        JREF["/joint_ref_states"]
        ACT["/action"]
        JSP["/joint_states"]
        IMUP["/imu"]
        SVC[标准服务接口]
    end

    subgraph NODE[InferenceNode 运行时]
        MAIN[Main Thread<br/>SCHED_FIFO p50]
        INF[Inference Thread<br/>SCHED_FIFO p70]
        CTRL[Control Thread<br/>SCHED_FIFO p70]
    end

    subgraph HW[RobotInterface 硬件抽象]
        IMU[IMU Driver<br/>坐标变换]
        MOT[Motor Driver<br/>ThreadPool 并行]
        DEC[CloseChain<br/> ankle_decouple_]
    end

    subgraph ML[模型与动作资产]
        ONNX[ONNX Runtime<br/>多策略 Session]
        MOTION[MotionLoader<br/>.npz 参考动作]
    end

    JOY --> MAIN
    CMD --> MAIN
    ELEV --> MAIN
    JREF --> MAIN
    MAIN -->|配置/服务| SVC

    MAIN -->|观测数据更新| INF
    INF -->|读取状态| HW
    MOTION --> INF
    INF -->|ONNX 推理| ONNX
    INF -->|写入 act_| CTRL
    CTRL -->|apply_action| HW
    HW --> ACT
    MAIN --> JSP
    MAIN --> IMUP

上图展示了从外部输入到电机执行的完整数据通路。主线程负责 ROS 2 事件循环与服务响应；推理线程以 dt * decimation 为周期采集传感器数据、组装观测张量并执行 ONNX 推理；控制线程则以基础周期 dt 运行，对推理输出的动作做平滑滤波后下发到硬件。两条工作线程均通过 SCHED_FIFO 调度策略绑定 70 号优先级，确保控制延迟的确定性。

Sources: inference_node.cpp robot_interface.cpp

核心模块与职责

Sources: inference_node.hpp robot_interface.hpp obs_manager.cpp

实时线程模型

系统采用三层线程架构，各自承担不同的实时性要求：

推理线程生成的动作通过互斥锁写入共享缓冲区 act_，控制线程以更高频率读取并平滑该缓冲区，从而在策略推理周期较长时仍保持电机指令的连续性与低抖动。此外，RobotInterface 内部还维护了一个按总线数量划分的 ThreadPool，用于并行执行多路 CAN/CANFD 总线上的电机数据回读与命令发送。

Sources: inference_node.cpp robot_interface.cpp

数据流与闭环

一条完整的控制闭环可概括为五个阶段：

1. 状态采集：RobotInterface 从 IMU 读取四元数与角速度（经外参旋转到机体坐标系），并从各总线电机读取关节位置、速度与电流；若存在闭链关节，则通过 Decouple 做正向运动学映射。
2. 外部输入融合：主线程实时更新手柄速度指令 cmd_vel_、外部感知 perception_obs_buffer_ 以及关节参考状态。
3. 观测组装与堆叠：推理线程根据当前策略的 obs_layout 调用对应观测源，将各字段拼接后按 frame_major 或 obs_major 策略进行帧堆叠，必要时追加 extra_obs_layout。
4. 模型推理与后处理：ONNX Runtime 执行单输入单输出推理，输出经裁剪（clip_actions）、缩放（action_scale）和默认角度偏移后映射到 URDF 关节顺序。
5. 动作平滑与执行：控制线程以更高频率对动作做平滑，最终通过 MIT 控制模式（pos, vel, kp, kd, tau）下发到电机；同时 RobotInterface 对闭链关节执行解耦力控。

Sources: inference_node.cpp obs_manager.cpp

项目结构

roboto_inference/
├── config/                 # 运行时 YAML 参数配置
│   ├── inference.yaml      # 主配置：模型、观测布局、缩放系数
│   ├── inference_amp.yaml  # AMP 风格策略配置示例
│   └── ...
├── include/
│   └── robot_interface.hpp # 硬件抽象对外接口
├── launch/
│   └── inference.launch.py # ROS 2 启动文件
├── models/                 # ONNX 策略模型仓库
│   ├── policy.onnx
│   └── policy_amp.onnx
├── motions/                # 参考动作序列（.npz）
│   ├── dance.npz
│   └── getup.npz
├── src/
│   ├── inference_node.cpp  # 节点主实现（线程/推理/生命周期）
│   ├── inference_node.hpp  # 节点头文件（ModelContext / PolicyRuntime）
│   ├── obs_manager.cpp     # 观测源注册与组装
│   ├── ros_interface.cpp   # ROS 参数/订阅/服务实现
│   ├── robot_interface.cpp # 电机与 IMU 硬件抽象实现
│   ├── pybind_module.cpp   # Python 绑定（脚本化控制入口）
│   └── utils/
│       ├── motion_loader.{hpp,cpp}
│       ├── close_chain_mapping.{hpp,cpp}
│       ├── decouple_atom01.{hpp,cpp}
│       └── thread_pool.hpp
└── thirdparty/             # 预编译依赖（ONNX Runtime / cnpy / yaml-cpp）
    ├── onnxruntime-linux-*
    ├── cnpy-linux-*
    └── yaml-cpp-0.8.0

Sources: CMakeLists.txt

配置分层体系

系统采用两层配置策略分离算法参数与硬件参数：

• ROS 参数层（config/*.yaml）：通过 inference.launch.py 加载到 InferenceNode，定义策略列表、观测布局（obs_layouts）、帧堆叠深度（frame_stacks）、归一化系数（obs_scales_*）、动作裁剪限幅（clip_actions）以及关节映射（usd2urdf）等。同一套二进制可配合不同 YAML 文件启动为行走、舞蹈、起身等不同行为模式。
• 硬件参数层（config/robot.yaml）：由 RobotInterface 直接解析，包含电机 ID、总线接口类型、零偏（motor_zero_offset）、阻抗参数（kp/kd）、闭链电机编号（close_chain_motor_id）以及 IMU 外参旋转矩阵（extrinsic_R）等。该文件与具体机器人本体强相关，通常不随策略切换而改变。

Sources: ros_interface.cpp robot_interface.cpp inference.yaml

下一步阅读建议

本页仅提供系统级鸟瞰，若需深入各子系统的实现细节，建议按以下路径继续阅读：

1. 若关注实时线程如何协同、优先级如何设置以及 over-run 如何处理，请阅读 实时双线程设计与调度策略。
2. 若关注多策略加载、运行时切换逻辑以及 motion policy 与 interrupt 的交互，请阅读 多策略运行时与热切换机制。
3. 若关注 ONNX Runtime 的 Session 创建、内存布局与图优化选项，请阅读 ONNX Runtime 推理引擎集成。
4. 若关注观测值从原始传感器到模型输入张量的完整变换链路，请阅读 观测值组装、归一化与帧堆叠。
5. 若关注电机驱动、CAN/CANFD 通信与 IMU 坐标变换，请阅读 RobotInterface 设计与实现 和 电机驱动与 CAN/CANFD 通信。
6. 若关注 ROS 2 话题接口与服务定义，请阅读 ROS 2 话题与服务接口。