系统架构总览

本文档从第一性原理出发，呈现 ATOM01 部署框架的整体架构设计、核心组件边界与数据流转关系。阅读后你将理解：推理节点为何采用双线程实时架构、硬件抽象层如何屏蔽电机与 IMU 的差异、以及配置文件如何驱动整个系统的行为。若你刚接触本项目，建议先阅读项目概览与快速开始建立上下文，再深入本节。

Sources: README_CN.md

架构全景图

系统以 ROS2 Humble 为中间件，采用三层隔离设计：应用层负责策略推理与状态机，硬件抽象层统一机器人本体表示，驱动层通过工厂模式对接不同电机与 IMU 的私有协议。下方的组件关系图展示了各模块的归属层次与数据依赖方向。

graph TB
    subgraph "应用层 Application Layer"
        IN["inference_node<br/>(roboto_inference)"]
        JN["joy_node / 感知节点<br/>(ROS2 生态)"]
    end

    subgraph "硬件抽象层 HAL"
        RI["RobotInterface"]
        ML["MotionLoader"]
        DC["Decouple<br/>(闭链解耦)"]
    end

    subgraph "驱动层 Driver Layer"
        MD["MotorDriver<br/>工厂模式"]
        ID["IMUDriver<br/>工厂模式"]
    end

    subgraph "协议层 Protocol Layer"
        SC["socket_can"]
        SCFD["socket_canfd"]
        SP["serial_port"]
    end

    subgraph "物理硬件 Hardware"
        MOT["伺服电机群<br/>(DM / EVO / LRO)"]
        IMU["IMU 传感器<br/>(HIPNUC)"]
    end

    JN -->|/joy / /cmd_vel / elevation| IN
    IN <-->|ROS2 Topics / Services| RI
    RI -->|apply_action / get_feedback| MD
    RI -->|get_quat / get_ang_vel| ID
    RI --> ML
    RI --> DC
    MD --> SC
    MD --> SCFD
    ID --> SP
    SC --> MOT
    SCFD --> MOT
    SP --> IMU

图中箭头方向代表调用或数据流向。应用层不直接操作硬件总线，所有传感器读取与执行器写入均通过 RobotInterface 收口；驱动层内部的 MotorDriver::create_motor 与 IMUDriver::create_imu 工厂函数则根据配置文件中的类型字符串在运行时实例化对应的具体驱动。这种分层使得新增一款电机或 IMU 时，无需修改推理节点源码，只需在驱动层新增子类并在工厂中注册即可。

Sources: inference_node.hpp, robot_interface.hpp, motor_driver.cpp, imu_driver.hpp

包层级结构与职责边界

仓库在 src/ 下划分了三个独立的 ROS2 包与一个工具目录，每个包拥有独立的 CMakeLists.txt、package.xml 与安装规则，可单独编译成 Debian 包部署。下表从职责、对外接口与关键依赖三个维度进行对比。

包名	核心职责	对外暴露的 C++ 接口	关键依赖
`roboto_inference`	ONNX 实时推理、观测堆叠、多策略切换、ROS2 节点生命周期	`InferenceNode`, `RobotInterface`	`rclcpp`, `onnxruntime`, `Eigen3`, `roboto_motors`, `roboto_imu`
`roboto_motors`	电机驱动抽象、MIT 控制指令封装、CAN/CANFD 总线通信	`MotorDriver` (工厂基类)	`Boost`, `spdlog`, `fmt`, `Eigen3`
`roboto_imu`	IMU 驱动抽象、姿态与角速度解析、串口/CAN 通信	`IMUDriver` (工厂基类)	`spdlog`, `fmt`

roboto_inference 作为上层包，在 CMakeLists.txt 中通过 find_package(roboto_motors REQUIRED) 与 find_package(roboto_imu REQUIRED) 显式声明对下层包的构建依赖，并在 package.xml 中以 <depend> 标签描述运行时依赖。电机包与 IMU 包自身不依赖 ROS2，仅当检测到 ament_cmake 时才生成 ROS2 相关的导出规则，因此它们也可被非 ROS2 的独立程序链接使用。

Sources: inference/CMakeLists.txt, inference/package.xml, motors/CMakeLists.txt, imu/CMakeLists.txt

核心推理系统：InferenceNode 线程模型

InferenceNode 是整个框架的中央处理单元，它继承自 rclcpp::Node，在生命周期内维护三条并发执行路径：主线程运行 ROS2 MultiThreadedExecutor 处理话题回调与服务请求；inference 线程以 dt × decimation 为周期执行 ONNX 模型推理；control 线程以 dt 为周期将最新动作写入电机。两条实时线程均通过 pthread_setschedparam 设置为 SCHED_FIFO 调度策略，优先级为 70，主线程优先级为 50。

sequenceDiagram
    participant MAIN as Main线程<br/>(Executor)
    participant INF as Inference线程<br/>(SCHED_FIFO 70)
    participant CTRL as Control线程<br/>(SCHED_FIFO 70)
    participant RI as RobotInterface
    participant ONNX as ONNX Runtime

    Note over MAIN: 处理 /joy, /cmd_vel,<br/>elevation, ROS2 Services

    loop 周期 = dt × decimation
        INF->>RI: get_joint_q / get_joint_vel<br/>get_quat / get_ang_vel
        RI-->>INF: 原始反馈数据
        INF->>INF: 更新观测段<br/>update_obs_segments
        INF->>INF: 时序堆叠<br/>update_stacked_obs
        INF->>ONNX: Session::Run()
        ONNX-->>INF: output_buffer[]
        INF->>INF: clip → scale → +default_angle<br/>写入 act_[]
    end

    loop 周期 = dt
        CTRL->>RI: apply_action(last_act_)
        RI->>RI: 读取电机反馈 + 闭链解耦
        RI->>RI: motor_mit_cmd(...)
    end

inference 线程与 control 线程通过 act_mutex_ 保护共享的 act_ 向量，避免数据竞争。控制线程在每次循环结束时使用一阶低通滤波更新实际下发值：last_act_ = act_alpha_ * act_ + (1 - act_alpha_) * last_act_，其中 act_alpha_ 由配置参数指定。若推理线程超时，系统会打印警告日志但不阻塞控制线程，保证硬件控制周期的确定性。

Sources: inference_node.cpp, inference_node.hpp

硬件抽象层：RobotInterface

RobotInterface 是连接高层推理与底层驱动的唯一网关，其构造函数接收 robot.yaml 路径，一次性完成 IMU 初始化、电机实例化、关节映射表构建与外参矩阵加载。它内部维护三个核心配置结构体：IMUCfg、MotorsCfg 与 RobotCfg，分别对应 YAML 中的 imu、motors、robot 三个根节点。

在关节映射方面，RobotInterface 处理了训练坐标系（USD/URDF）与电机物理编号之间的双向转换。urdf2motor_ 数组定义了 URDF 关节索引到电机实例索引的映射，而 motor2urdf_ 则是其反向查找表；motor_sign_ 用于校正电机安装方向导致的正负号反转。当上层推理节点调用 get_joint_q() 时，RobotInterface 先读取各电机的原始位置，再乘以 motor_sign_ 并按 motor2urdf_ 重排，最终返回与 URDF 顺序一致的关节向量。

对于闭链机构（如踝关节的四连杆），RobotInterface 在 apply_action() 中调用 ankle_decouple_->get_forwardQVT() 将电机空间的位置/速度/力矩转换到关节空间，并在下发前调用 get_decoupleQVT() 将关节空间的力矩目标反解回电机空间。这一解耦逻辑对上层完全透明，上层始终工作在关节空间。

Sources: robot_interface.cpp, robot_interface.cpp, robot_interface.hpp

驱动层架构：工厂模式与协议栈

电机驱动与 IMU 驱动均采用抽象基类 + 工厂方法的设计。MotorDriver 定义了统一的控制原语，包括 motor_mit_cmd、refresh_motor_status、set_motor_zero 等纯虚接口；IMUDriver 则定义了 get_quat、get_ang_vel、get_lin_acc 等标准数据获取接口。工厂函数根据配置中的类型字符串在运行时完成实例化，当前已支持的电机类型包括 DM、EVO 与 LRO，IMU 类型包括 HIPNUC。

graph TD
    A[MotorDriver<br/>抽象基类] -->|继承| B[DmMotorDriver]
    A -->|继承| C[EvoMotorDriver]
    A -->|继承| D[LroMotorDriver]

    B --> E[SocketCAN / CAN]
    B --> F[SocketCANFD / CANFD]
    C --> E
    C --> F
    D --> E
    D --> F

    G[IMUDriver<br/>抽象基类] -->|继承| H[HipnucIMUDriver]
    H --> I[SerialPort]
    H --> J[SocketCAN]

协议层与驱动层解耦：电机侧的 SocketCAN 与 SocketCANFD 封装了 Linux SocketCAN 接口，负责帧打包与总线收发；IMU 侧的 SerialPort 与 SocketCAN 负责字节流解析。驱动子类专注于将厂商特定的数据格式转换为统一语义。例如 DM 电机使用 MIT 控制模式，其 motor_mit_cmd 实现负责将 pos/vel/kp/kd/tau 五元组编码为 CAN 帧，而协议层仅提供原始字节的发送与接收。若需扩展新电机类型，只需继承 MotorDriver 并实现编码逻辑，无需修改 SocketCAN 代码。

Sources: motor_driver.hpp, motor_driver.cpp, imu_driver.hpp

数据流与实时控制管线

从传感器采样到执行器响应的完整数据流可分为感知、推理、执行三个阶段。感知阶段由 RobotInterface 在 control 线程每次调用 apply_action 时隐式完成：它先向所有电机发送状态刷新指令，读取返回的位置、速度与电流，再经闭链解耦与坐标变换后缓存到 joint_q_、joint_vel_、joint_tau_ 中；IMU 数据则通过 get_quat() 与 get_ang_vel() 在调用时实时读取并施加外参旋转矩阵。

推理阶段由 inference 线程驱动。InferenceNode 维护一个观测源注册表 obs_source_definitions，内置了 ang_vel、gravity_b、cmd_vel、dof_pos、dof_vel、last_action、perception、interrupt、motion_pos、motion_vel 等标准观测项。配置参数 obs_layouts 以 "name:size" 的逗号分隔语法定义每个策略所需的观测向量结构，系统在运行时通过指针到成员函数的映射将字符串名称绑定到具体的数据采集函数，从而避免硬编码观测顺序。

执行阶段发生在 control 线程。last_act_ 经低通滤波后被送入 RobotInterface::apply_action，后者区分普通关节与闭链关节：普通关节直接以 MIT 模式下发 pos=target, kp=cfg.kp, kd=cfg.kd；闭链关节则以下发计算后的力矩 tau=target 工作。当总线类型为 CANFD 时，RobotInterface 利用内部线程池 ThreadPool 并行向各总线发起 one-to-many 批量指令，显著降低多电机场景下的通信延迟。

Sources: obs_manager.cpp, inference_node.cpp, robot_interface.cpp, thread_pool.hpp

通信中间件与 DDS 优化

系统内部使用 ROS2 话题进行异步数据交换，使用 ROS2 服务进行同步控制请求。InferenceNode 订阅的话题包括手柄输入 /joy（sensor_msgs/msg/Joy）、速度指令 /cmd_vel（geometry_msgs/msg/Twist）、外部感知 elevation_data（std_msgs/msg/Float32MultiArray）以及参考关节状态 /joint_ref_states（sensor_msgs/msg/JointState）。发布的话题包括推理动作 /action、IMU 数据 /imu 与当前关节状态 /joint_states。所有话题均使用 KeepLast(1) 配合 best_effort 与 durability_volatile 的 QoS 配置，确保实时系统中只处理最新样本，避免历史消息堆积。

服务接口共 10 个，均基于 std_srvs/srv/Trigger 类型，涵盖电机使能（init_motors / deinit_motors）、关节复位（reset_joints）、零点标定（set_zeros）、错误清除（clear_errors）、状态刷新（refresh_joints）、传感器读取（read_joints / read_imu）以及推理启停（start_inference / stop_inference）。这些服务在节点构造函数中一次性创建，回调函数内通过原子变量与互斥锁与实时线程安全交互。

为降低 DDS 发现延迟与内存拷贝开销，启动脚本 start_robot.sh 强制将 RMW_IMPLEMENTATION 设为 rmw_fastrtps_cpp，并加载 assets/rt_fastdds_profile.xml 中的共享内存传输配置。脚本还会通过检查 /proc/$pid/environ 中的环境变量以及 /dev/shm/ 下的共享内存文件数量来验证 DDS 配置是否真正生效。

Sources: inference_node.hpp, ros_interface.cpp, start_robot.sh

观测系统与多策略配置

InferenceNode 支持在单个节点内加载并热切换多个 ONNX 策略模型。每个策略在内部被表示为 PolicyRuntime 结构，包含独立的 ModelContext（ONNX 会话与内存管理器）、观测布局 obs_layout、时序堆叠深度 frame_stack 以及可选的运动序列加载器 motion_loader。配置参数 model_names、obs_layouts、frame_stacks、obs_stack_orders 为等长数组，数组索引即策略索引。

观测时序堆叠支持两种内存排布策略：FrameMajor 按 (frame_stack, obs_num) 扁平化，ObsMajor 则按观测字段分组堆叠，每种字段内部再按时间帧展开。后者在字段尺寸不一致时更为紧凑。update_stacked_obs 函数根据策略指定的 stack_order 执行滑动窗口更新：首帧时填充全部历史窗口，后续帧则以 std::move 左移旧数据并追加最新观测，避免每帧重新分配内存。

若策略配置了 motion_path，则系统会在推理时从 MotionLoader 中按帧索引读取参考位置与参考速度，作为额外的观测输入注入模型。这一机制支持模仿学习与动作复现模式。策略切换通过手柄 LB 键或 ROS2 服务触发，切换时当前策略的运行时状态会被重置，但硬件接口保持连续。

Sources: inference_node.hpp, inference_node.cpp, motion_loader.hpp

Python 绑定层

三个核心包均通过 pybind11 向 Python 暴露了关键接口，使得用户可以在不编译完整 ROS2 节点的情况下直接操作硬件或运行推理逻辑。roboto_inference 编译出 robot_py 模块，暴露 RobotInterface 类的构造、动作下发、电机使能、传感器读取等接口；roboto_motors 编译出 motors_py 模块；roboto_imu 编译出 imu_py 模块。这些 Python 模块在 colcon build 后被安装到 lib/pythonX.Y/site-packages 目录下，通过 import robot_py 即可调用。

Sources: pybind_module.cpp, inference/CMakeLists.txt

配置体系

系统通过两组 YAML 文件实现硬件属性与算法行为的正交分离。robot.yaml 描述机器人本体的静态物理属性，包括 IMU 的串口号与波特率、各电机的总线分配与编号、关节方向符号、闭链电机 ID 列表、刚度阻尼系数 kp/kd 以及 IMU 外参旋转矩阵。inference.yaml 描述推理算法的动态行为参数，包括模型文件名、观测字段布局、时序堆叠深度 frame_stack、控制周期 dt、推理倍频 decimation、观测缩放系数、动作裁剪范围以及默认关节角度。

这种分离意味着：更换机器人本体时只需修改 robot.yaml；更换训练策略或调整观测窗口时只需修改 inference.yaml。启动文件 inference.launch.py 将 inference.yaml 以 ROS2 parameters 的形式加载到 inference_node 中，节点在构造函数内通过 declare_parameter 与 get_parameter 完成参数解析与校验。

Sources: robot.yaml, inference.yaml, inference.launch.py