本节聚焦推理节点与底层执行器之间的硬件抽象层，解析 RobotInterface 如何通过多总线拓扑管理 23 个关节电机、如何在 CAN 与 CANFD 两种物理层协议之间切换发包策略，以及实时线程池如何按总线维度并行调度以保证控制周期不被通信阻塞。所有电机驱动实体由外部包 roboto_motors 通过工厂模式创建，本层仅负责总线编排、状态聚合与安全监控。

多总线拓扑与电机配置

RobotInterface 在构造阶段从 config/robot.yaml 加载 motors 节点，映射为 MotorsCfg 结构。与单总线设计不同，本系统采用多 CAN/CANFD 总线分域架构：每条总线绑定一个 Linux SocketCAN 设备（如 can0），并承载固定数量的电机。以 Atom01 默认配置为例，23 个电机被划分为 4 组，分别挂载到 can0~can3，组内电机数依次为 6、7、5、5。这种拓扑既分散了总线负载，也将电气故障隔离在单条总线域内。

MotorsCfg 关键字段说明如下：

setup_motors() 按双重循环实例化驱动：外层遍历总线，内层按 motor_num[i] 计数，依次调用 MotorDriver::create_motor()。工厂根据 motor_interface_type（"can" 或 "canfd"）以及 motor_type、motor_model 返回对应协议栈的驱动对象。所有驱动句柄存入 motors_ 向量，其下标与 motor_id 一一对应，后续通过 motor2urdf_ 与 urdf2motor_ 完成 URDF 关节索引与物理电机索引的双向映射。

Sources: robot_interface.hpp, robot_interface.cpp, robot.yaml

MotorDriver 抽象与工厂契约

MotorDriver 定义在独立包 roboto_motors 中，被本仓库以 find_package(roboto_motors REQUIRED) 引入。RobotInterface 并不直接感知底层寄存器细节，而是通过统一的虚接口与驱动交互。工厂方法签名如下：

MotorDriver::create_motor(
    motor_id, motor_interface_type, motor_interface,
    motor_type, motor_model, master_id_offset, motor_zero_offset
)

返回的共享指针对外暴露以下核心操作（由 RobotInterface 实际调用）：

• init_motor() / deinit_motor()：使能 / 去使能功率级
• motor_mit_cmd(...)：下发 MIT 模式控制帧（位置、速度、kp、kd、力矩）
• refresh_motor_status()：强制刷新电机内部状态缓存
• get_motor_pos() / get_motor_spd() / get_motor_current()：读取状态
• get_response_count()：获取连续未响应计数，用于离线判定
• set_motor_zero() / clear_motor_error()：零位标定与故障清除

RobotInterface 持有 std::vector<std::shared_ptr<MotorDriver>> motors_，所有高层控制命令均以 lambda 形式遍历该向量，并通过 exec_motors_parallel() 按总线维度并发执行。

Sources: robot_interface.hpp, robot_interface.cpp, CMakeLists.txt

CAN 与 CANFD 双模式通信协议

本系统同时支持传统 CAN（1 Mbps 级）与 CANFD（最高 8 Mbps 数据段）两种物理层协议，切换仅通过修改 motor_interface_type 字段完成，无需改动业务代码。但两种协议在发包粒度与批量策略上存在本质差异，apply_action() 内部通过条件分支分别处理。

CAN 模式：单电机逐包并行

当 motor_interface_type == "can" 时，每个电机独立构造一帧 MIT 命令，包含目标位置、速度、kp、kd、力矩共 5 个浮点量。exec_motors_parallel() 将电机按总线分组后，各总线任务串行调用该总线所属电机的 motor_mit_cmd()。虽然单帧数据量小，但 CAN 标准帧每帧 8 字节有效载荷的限制导致需要多帧拆分或高频率发包，因此并行化主要目的是降低单总线内部串行延迟，而非跨总线带宽聚合。

motor->motor_mit_cmd(
    motor_target_[idx] * robot_cfg_->motor_sign_[idx],  // pos
    0.0f,                                                // vel
    robot_cfg_->kp_[idx],                                // kp
    robot_cfg_->kd_[idx],                                // kd
    0.0f                                                 // tau
);

CANFD 模式：总线级批量打包

当 motor_interface_type == "canfd" 时，系统利用 CANFD 的单帧高 payload 特性，将整条总线上的全部电机目标值打包进一个数组，通过一次 motor_mit_cmd() 调用完成批量下发。apply_action() 为每条总线构造 5 个长度为 8 的数组（pos[8], vel[8], kp[8], kd[8], tau[8]），数组下标由 motor_id - 1 映射得到。非闭环电机写入位置+刚度命令，闭环脚踝电机则写入力矩命令。

float pos[8] = {}, vel[8] = {}, kp[8] = {}, kd[8] = {}, tau[8] = {};
// 按 motor_id 填入对应 slot
motors_[start_count]->motor_mit_cmd(pos, vel, kp, kd, tau);

这一设计将 N 次系统调用与帧发送压缩为 1 次，显著降低内核态切换开销，是高频率实时控制的关键优化。

Sources: robot_interface.cpp

实时并行调度与线程池设计

电机通信必须避免阻塞主控制线程，因此 RobotInterface 引入 ThreadPool 实现按总线维度的任务并行。线程池大小在构造时显式指定为 motor_interface_.size()，即总线数量，这意味着系统为每条总线维护一个专用工作线程。

线程池中的工作线程均通过 pthread_setschedparam 设置为 SCHED_FIFO 优先级 70，与 inference 线程、control 线程同级，确保通信任务在 kernel 调度层面不会因普通线程抢占比而饥饿。

exec_motors_parallel() 是并行调度的核心模板：它接收一个电机级 lambda，随后按 motor_num 将电机切分为总线级任务块，每个任务块内串行遍历所属电机并执行 lambda。最终调用 thread_pool_->run_parallel(tasks) 等待全部总线任务完成。该模式被复用于状态读取、初始化、去初始化、零位设置、故障清除等所有电机操作。

flowchart TD
    A[exec_motors_parallel<br/>cmd_func] --> B{"按总线切分<br/>motor_num[i]"}
    B -->|总线0: can0<br/>电机0~5| C[ThreadPool Worker 0]
    B -->|总线1: can1<br/>电机6~12| D[ThreadPool Worker 1]
    B -->|总线2: can2<br/>电机13~17| E[ThreadPool Worker 2]
    B -->|总线3: can3<br/>电机18~22| F[ThreadPool Worker 3]
    C --> G[run_parallel<br/>wait_all]
    D --> G
    E --> G
    F --> G

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

周期控制循环与状态反馈

InferenceNode 的 control 线程以固定周期（由 dt_ 决定，典型值 2 ms）循环调用 robot_->apply_action()。该函数是电机驱动层的核心周期入口，其内部执行流程可分为四个阶段：

1. 状态采样：在 joint_mutex_ 保护下，通过 exec_motors_parallel() 并行读取所有电机的 pos、spd、current，并按 motor_sign_ 做方向校正，映射到 URDF 关节空间。
2. 离线检测：若某电机的 get_response_count() > offline_threshold_（默认 25），立即抛出 runtime_error，由上层 control 线程捕获后执行 rclcpp::shutdown()，防止失控。
3. 闭环解耦（ ankle ）：对 close_chain_motor_idx_ 中的脚踝电机，先通过 Decouple 做正运动学映射，再计算 PD 力矩，最后做逆映射，生成力矩目标值。闭环关节在此阶段以力矩模式运行，不接收位置目标。
4. 命令下发：将策略输出的动作向量转换到电机空间，按 CAN/CANFD 分支调用对应发包逻辑。

apply_action 的调用关系与数据流如下图所示：

sequenceDiagram
    participant CTL as Control Thread<br/>(SCHED_FIFO 70)
    participant RI as RobotInterface
    participant TP as ThreadPool
    participant MD as MotorDriver (per bus)

    CTL->>RI: apply_action(action)
    RI->>TP: exec_motors_parallel(read status)
    TP->>MD: get_motor_pos/spd/current
    MD-->>TP: raw state
    TP-->>RI: joint_q_, joint_vel_, joint_tau_
    RI->>RI: offline check & decouple
    RI->>TP: exec_motors_parallel(send cmd)
    TP->>MD: motor_mit_cmd(...)
    MD-->>TP: OK/FAIL
    TP-->>RI: done
    RI-->>CTL: return

对于非推理阶段的辅助操作，RobotInterface 还暴露以下接口：

• reset_joints()：分两个阶段下发默认角度，第一阶段刚度除以 2.5 运行 1 秒，第二阶段恢复全刚度，避免瞬间大电流冲击。
• refresh_joints()：强制所有电机刷新状态缓存并回读，常用于标定或诊断。
• set_zeros() / clear_errors()：全电机并行执行零位标定与故障清除。

Sources: robot_interface.cpp, inference_node.cpp, robot_interface.cpp

安全监控与互斥策略

电机驱动层涉及三类并发参与者：Control 线程（写命令）、Inference 线程（读关节状态用于观测组装）、以及 ROS 2 Service 回调（如 refresh_joints_srv）。RobotInterface 通过两把细粒度互斥锁隔离资源竞争：

• joint_mutex_：保护关节状态缓冲区 joint_q_、joint_vel_、joint_tau_。apply_action() 在采样阶段上锁，ROS 2 Service 读关节时同样上锁，保证观测值的一致性。
• motors_mutex_：保护电机命令缓冲区 motor_target_ 及底层驱动访问序列。apply_action() 在写目标值与下发命令时上锁，防止 Service 回调（如 reset_joints）与周期控制并发修改同一电机。

离线检测机制是安全链的最后一环。底层驱动维护每个电机的无响应计数，当物理总线断开、电机掉电或 CAN 收发器故障时，该计数持续累加。apply_action() 在每次周期采样后检查阈值，一旦超限立即抛出异常。由于 Control 线程以 SCHED_FIFO 运行，异常会立刻终止整个节点进程，依赖看门狗或 systemd 的 Restart=on-failure 策略进入安全状态。

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

延伸阅读与导航

电机驱动层之上，InferenceNode 的 Control 线程以固定周期调用 apply_action()，而 Inference 线程负责策略推理与观测组装，两者协同构成完整的实时双线程架构，详见 实时双线程设计与调度策略。若要了解电机状态如何被采集成观测向量并送入 ONNX 模型，请参考 观测值组装、归一化与帧堆叠。对于脚踝闭环机构的运动学解耦数学，可查阅 闭环解耦与运动学映射。