ros2_control 硬件在环：仿真与实机无缝切换

同一个 joint_trajectory_controller，今天在 Gazebo 里驱动一组理想电机，明天通过 CAN 总线驱动十二个真关节——中间只改一个 launch 参数。这不是宣传语，这是 ros2_control 圈子里每天都在发生的事。

这篇讲清楚这层"插座标准"是怎么设计的：为什么能换、怎么写自己的插头、以及 1 kHz 实时循环里哪些东西碰不得。每一层抽象都在消灭一种分叉，而分叉，是机器人软件一切玄学 Bug 的源头。

一、起点：仿真里跑通了，实机上重写一遍

传统做法长这样：仿真时写一个节点订阅 Gazebo 的 /joint_states、往 /joint_commands 发目标；接实机那天，重写一个节点订阅串口、发 CAN 帧。功能一样，代码两套。

两套代码不是双保险，是双债务。接口对不齐、单位对不齐、时序更对不齐——仿真里毫秒级抖动无所谓，实机上就是轨迹毛刺。更隐蔽的代价在验证链路：仿真里修好的每一个 Bug，都得在实机代码里再修一遍，而且没人监督你修得一致。三个月后两边各自打过补丁，Bug 只在实机出现，仿真彻底失去预测能力，调试只能上真机，烧的是电机，冒的是烟。

ros2_control 的承诺很朴素：在控制器和硬件之间插一层硬件抽象层。控制器只跟"关节的位置/速度/力矩接口"打交道，不关心接口背后是物理引擎还是真电机。换硬件换的是插头，不是重新布线整栋房子。

二、三层积木：Controller Manager、Controller、Hardware Interface

整个框架就三层，外加一个 1 kHz 的节拍器（Controller Manager 的实时线程）。每拍干三件事：read() 把硬件状态搬进来，update() 让控制器算，write() 把命令搬出去。三件事在同一个线程里顺序执行——没有锁、没有队列、没有跨核抖动，这是它能跑 1 kHz 的根本原因。

       controller_manager（实时线程，1 kHz 节拍器）
┌─────────────────────────────────────────────────┐
│    每一拍：read() ──▶ update() ──▶ write()       │
│                                                 │
│  Controller 层（标准件，与硬件无关）              │
│  ├─ JointTrajectoryController  ◀── MoveIt 轨迹  │
│  └─ joint_state_broadcaster  ──▶ /joint_states  │
│         │ command_interface      ▲ state_iface  │
│         ▼                         │             │
│  Hardware Interface 层（插件，唯一可替换层）      │
│  ├─ gazebo_ros2_control/GazeboSystem            │
│  └─ spider_hardware/SpiderBoardHardware         │
└────────────┬──────────────────────┬─────────────┘
             ▼                      ▼
      Gazebo 物理引擎        CAN 总线 → FOC 驱动板

最下面那层是唯一可替换层，靠 pluginlib 按名字动态加载，名字写在 URDF 的 <ros2_control> 标签里——这就是第四节那个切换开关的物理位置。上面两层对插件切换完全无感：MoveIt 不知道也没必要知道轨迹最终发给了谁。

接口还有租约机制：控制器加载时必须声明自己要占用的 command_interface，两个控制器抢同一个关节的写权会被直接拒绝。手柄遥操作和轨迹执行可以并存，但谁也不能同时握住同一个关节——冲突在加载期就炸出来，而不是在运行期打架。

控制器和硬件组件都实现了生命周期，状态迁移全靠回调：

unconfigured ──on_configure──▶ inactive ──on_activate──▶ active
     ▲                            │  ▲                     │
     └─────────on_cleanup─────────┘  └────on_deactivate────┘

一个值得记住的细节：inactive 态下 read() 照常执行（关节状态持续可见），但 write() 不会把命令下发给硬件——先看到真实状态，再谈使能，这是框架内建的安全语义。部署脚本因此可以分两步走：先把硬件拉到 inactive，肉眼核对关节角度无误，再 on_activate 上使能。

三、动手：把 FOC 驱动板包成一个插件

自定义硬件接口就是继承 SystemInterface 实现四个函数。下面是一个 12 关节 CAN 总线驱动板的骨架（Iron/Jazzy API；Humble 里导出函数叫 export_state_interfaces，没有 on_ 前缀）：

#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"

namespace spider_hardware
{

// 一块 CAN 总线 FOC 驱动板带 12 个关节：SystemInterface = 一个系统多关节
class SpiderBoardHardware : public hardware_interface::SystemInterface
{
public:
  // 生命周期第一步：上电自检。还没进实时线程，允许阻塞
  hardware_interface::CallbackReturn on_init(
    const hardware_interface::HardwareInfo & info) override
  {
    if (SystemInterface::on_init(info) !=
      hardware_interface::CallbackReturn::SUCCESS)
    {
      return hardware_interface::CallbackReturn::ERROR;
    }
    hw_positions_.resize(12, 0.0);  // 速度、命令向量同理
    // 关节数与实物对账 + 打开 CAN（设备名来自 URDF <param> 标签）：
    // 任一步失败都拒绝启动，别带病上电
    if (info_.joints.size() != 12 ||
        !driver_.open(info_.hardware_parameters.at("can_interface")))
    {
      return hardware_interface::CallbackReturn::ERROR;
    }
    return hardware_interface::CallbackReturn::SUCCESS;
  }

  // 向框架声明“能读什么”：每个关节 position + velocity 状态接口
  std::vector<hardware_interface::StateInterface> on_export_state_interfaces() override
  {
    std::vector<hardware_interface::StateInterface> states;
    for (size_t i = 0; i < info_.joints.size(); ++i)
    {
      states.emplace_back(info_.joints[i].name,
        hardware_interface::HW_IF_POSITION, &hw_positions_[i]);
      states.emplace_back(info_.joints[i].name,
        hardware_interface::HW_IF_VELOCITY, &hw_velocities_[i]);
    }
    return states;
  }
  // on_export_command_interfaces 与之完全对称：
  // 每个关节导出一个 HW_IF_POSITION 命令接口，指向 &hw_commands_[i]

  // 实时循环上半场：只从无锁环形缓冲拷贝编码器数据，绝不等待
  hardware_interface::return_type read(
    const rclcpp::Time &, const rclcpp::Duration &) override
  {
    driver_.drain_rx_ringbuffer();  // 通信线程在后台填
    for (size_t i = 0; i < 12; ++i)
    {
      hw_positions_[i] = driver_.last_position(i);
      hw_velocities_[i] = driver_.last_velocity(i);
    }
    return hardware_interface::return_type::OK;
  }

  // 实时循环下半场：命令写进 TX 缓冲由通信线程发出，不等 ACK
  hardware_interface::return_type write(
    const rclcpp::Time &, const rclcpp::Duration &) override
  {
    for (size_t i = 0; i < 12; ++i)
      driver_.push_target(i, hw_commands_[i]);
    return hardware_interface::return_type::OK;
  }

private:
  SpiderCanDriver driver_;  // 你自己的 FOC 板通信层
  std::vector<double> hw_positions_, hw_velocities_, hw_commands_;
};

}  // namespace spider_hardware

// 注册插件：URDF <plugin> 标签引用的就是这个名字
PLUGINLIB_EXPORT_CLASS(spider_hardware::SpiderBoardHardware,
  hardware_interface::SystemInterface)

注意导出接口时的写法：emplace_back 传的是裸指针，框架和你的成员向量共享同一块内存——控制器写命令、你读命令，零拷贝。这也是为什么成员向量只在 on_init 里 resize 一次、之后绝不再动：实时阶段不改布局，是零拷贝成立的前提。

代码之外，四条纪律比代码本身更重要：

on_init 可以阻塞（开设备、握参数），read()/write() 绝对不行——它们跑在实时线程里，等一次锁就是全机器人卡一拍。
命令接口类型要对齐驱动器能力：导出 position 接口，意味着位置环在板上跑；板子只会发力矩就导出 effort，别在 write() 里偷偷补一个 PID。
通信全部走"后台线程 + 无锁环形缓冲"，read()/write() 只碰内存。
URDF 里声明的关节和实物对不上就返回 ERROR，宁可拒绝启动。

四、切换：一个 launch 参数，两条世界线

URDF 里的 <ros2_control> 标签声明"这套关节暴露哪些接口、用哪个插件"——它和连杆、惯量写在同一个文件里，是机器人描述的一部分。仿真与实机的全部区别，被压缩到 plugin 名字这一个字段：

<!-- spider.urdf.xacro：控制接口声明与物理描述写在同一个文件里 -->
<ros2_control name="SpiderBot" type="system">
  <hardware>
    <!-- 仿真与实机的全部区别，就这个 plugin 字段 -->
    <xacro:if value="$(arg use_sim)">
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </xacro:if>
    <xacro:unless value="$(arg use_sim)">
      <plugin>spider_hardware/SpiderBoardHardware</plugin>
      <param name="can_interface">can0</param>  <!-- on_init 里读到它 -->
    </xacro:unless>
  </hardware>
  <joint name="joint_1">
    <command_interface name="position"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>  <!-- joint_2 ~ joint_12 同构，此处省略 -->
</ros2_control>

joint 标签里的接口声明对两个插件一视同仁——GazeboSystem 按声明对接物理引擎，SpiderBoardHardware 按声明对接驱动板。接口清单就是双方的契约，谁少声明一个，控制器加载时就会报错指名道姓。

launch 侧用 IfCondition/UnlessCondition 把两条世界线装进同一个文件：

# bringup.launch.py —— 仿真与实机共用这一个文件
def generate_launch_description():
    use_sim = LaunchConfiguration('use_sim')
    urdf = PathJoinSubstitution([FindPackageShare('spider_description'),
                                 'urdf', 'spider.urdf.xacro'])
    # 机器人描述两边共用，唯一变量 use_sim 从命令行进来
    common = {'robot_description': ParameterValue(
        Command(['xacro ', urdf, ' use_sim:=', use_sim]), value_type=str)}
    spawners = [Node(package='controller_manager', executable='spawner',
                     arguments=['joint_state_broadcaster',
                                'joint_trajectory_controller'])]  # 两边复用

    # 世界线 A：Gazebo——controller_manager 由 gz 插件在物理引擎内拉起
    sim_group = GroupAction(condition=IfCondition(use_sim), actions=[
        Node(package='robot_state_publisher', executable='robot_state_publisher',
             parameters=[common]),
        IncludeLaunchDescription(PythonLaunchDescriptionSource(
            PathJoinSubstitution([FindPackageShare('gazebo_ros'),
                                  'launch', 'gazebo.launch.py']))),
    ] + spawners)

    # 世界线 B：实机——ros2_control_node 加载 URDF，指到 SpiderBoardHardware
    real_group = GroupAction(condition=UnlessCondition(use_sim), actions=[
        Node(package='controller_manager', executable='ros2_control_node',
             parameters=[common, controllers_yaml]),
    ] + spawners)

    return LaunchDescription([
        DeclareLaunchArgument('use_sim', default_value='false',
                              description='true=Gazebo，false=实机硬件'),
        sim_group, real_group])

命令行切换就是一行：ros2 launch spider_bringup bringup.launch.py use_sim:=true，把 true 换成 false，跑的就已经是真机。controllers.yaml 两边共用同一份，update_rate: 1000 也是——Gazebo 会诚实地按 1 kHz 步长推进物理引擎，仿真里的时序行为对实机有参考价值。

增益这类参数不必强行共用：yaml 里给仿真和实机各留一份覆盖文件，launch 里按 use_sim 选——代码一份、标定分档，才是"无缝"的完整含义：逻辑不变，参数各认各的硬件。

这就是硬件在环（HIL）的全部机关：**上层代码零改动，被测系统的边界从物理引擎推到了真芯片。**你在仿真里验证的控制器增益、轨迹时序、看门狗逻辑，上实机时原封有效——因为跑的就是同一份代码。

五、选型：逆解插件与轨迹控制器

MoveIt 发轨迹之前要先解逆运动学，IK 插件同样是"可替换件"——换求解器不用动控制器一行代码。三个主流选手：

维度	KDL	Trac-IK	BioIK
求解策略	雅可比迭代	SQP 与 NLopt 双线程竞速	进化算法全局搜索
随机姿态可解率	约 68%（5 ms 截止）	约 99%	与 Trac-IK 相当，解更优
平均耗时	基准	快 1~3 倍	可配置，整体偏慢
多目标优化	不支持	不支持	支持：避障、姿态、关节权重
依赖成本	MoveIt 自带	编译 trac_ik	编译 bio_ik
适用构型	简单构型快速起步	6 轴机械臂默认首选	腿足、冗余构型

表中可解率来自 TRAC-IK 论文的批量测试——同一个机械臂、上万组随机目标姿态，KDL 在 5 ms 截止时间内只解出 68%，Trac-IK 靠两种算法并行赛跑拉到 99%。没有最好的求解器，只有匹配构型的求解器：6 轴臂用 Trac-IK 基本不会错；一解多形的冗余构型才轮到 BioIK，它的代价函数能把"避障""肘朝下""限位余量"塞进同一个优化目标。

轨迹这层的主角是 JointTrajectoryController。MoveIt 算出的轨迹只是一串带时间戳的路径点，JTC 负责点与点之间的样条插补：每个 1 ms 周期吐出一个插值点写进 command_interface。对上层它是"轨迹执行器"，对下层它只是每毫秒发一个位置目标——真正的位置环在 FOC 驱动板上，驱动器的性能上限就是整个系统的上限。

JTC 的参数表里值得先啃的是 constraints 容差和增益项：实机上调增益前，先看清板上位置环已经做了多少——软件插补层和驱动器级联两层高增益位置环，是实机振荡最经典的来源。

急停打断也在这层做：往 JTC 发一条新轨迹，首点等于当前实际位置、time_from_start 只有 10 ms，旧轨迹立刻被取代，机械臂在当前位置平滑刹停。这是软件层的最后一道闸——电路级硬急停仍然不可替代，两套都要有。

六、纪律：1000 微秒的收支表

1 kHz 意味着每个周期预算 1000 μs，read → update → write 三段加调度开销全在里面。这个预算薄到什么程度？一条 printf 打到终端可能吃掉几十到上百微秒——终端是慢设备，还带锁；一次动态内存分配可能触发页缺失，直接把这一拍的开销打穿。在实时线程里：printf 是奢侈品，new/delete 是违章建筑，阻塞等待是事故现场。

一组可参考的量级：12 关节的 read 拷贝约 15 μs，JTC 插补 update 约 40 μs，write 打包约 10 μs——名义占用不到一成，但预算表要按三倍余量规划，1 kHz 才经得起内核突发和最坏情况。

正确姿势：所有缓冲预分配、队列用无锁环形结构、CAN/串口收发放独立线程，read()/write() 只做内存拷贝。上面的骨架通篇没有一个锁、一次分配，不是巧合。剩下的预算大头留给 update() 里的控制器计算——但如果有人在控制器里塞了一个服务调用，整个系统就开始抖了。

另一半纪律在内核：普通 Linux 的调度延迟可达几十毫秒，节拍器再准也会被内核整体劫走一拍。打上 PREEMPT_RT 补丁后，最大抢占延迟典型值降到 100 μs 量级，cyclictest 可以实测验证。怎么打补丁、怎么用 Docker 把这套环境固化成可复现的镜像，是上一篇的主题。

七、写给 DIY 者：这套架构和你有什么关系

从官方 demo 插件抄起。ros2_control 仓库的 demo_examples 里有完整的 CMakeLists、plugin.xml 和 URDF，别从零搭骨架。
分层点亮：先 joint_state_broadcaster（RViz 里能看到关节转动），再上 JointTrajectoryController（能给目标位置了），最后才接 MoveIt。一上来全栈启动，错了不知道错在哪层。
在仿真里测实时性：把 update_rate 提到 2000 跑一晚上，控制器代码里若藏了阻塞调用，仿真里就能看出周期超支，别等实机抖了再回头找。
急停两套都要：软件层用"首点即当前位置"的短轨迹打断，硬件层的急停回路独立于一切软件存在。
实机首日守则：空载、低增益、手放在急停按钮上；on_activate 之前，先确认 inactive 态读回的关节角度和肉眼看到的一致。

参考文献

ros2_control 官方文档. https://control.ros.org/ （hardware interface 生命周期、接口类型与实时执行器）
ros-controls 社区. ros2_control GitHub 仓库. https://github.com/ros-controls/ros2_control （源码与 demo_examples 示例插件）
ros-controls 社区. gazebo_ros2_control GitHub 仓库. https://github.com/ros-controls/gazebo_ros2_control （Gazebo 侧的 hardware interface 插件）
Anderson, K. "TRAC-IK: An Open-Source Library for Improved Inverse Kinematics." TRACLabs, 2015. 仓库：https://github.com/ros-industrial-consortium/trac_ik （可解率对比数据来源）
MoveIt 2 官方文档，Kinematics 配置章节. https://moveit.picknik.ai/ （KDL / TRAC-IK / BioIK 插件配置）
The Linux Foundation. PREEMPT_RT wiki. https://wiki.linuxfoundation.org/realtime/ （实时内核补丁与延迟验证）

这套插拔架构要落到硅和铜上，还差两块地基：ROS 2 工业级环境构建：Docker 容器化与 PREEMPT_RT 实时内核——实时内核是那 1 kHz 节拍能兑现的前提；三相 FOC 驱动板：栅极驱动与电流采样——你 write() 最终写到的，就是那块板子。

本文采用署名-非商业性使用-相同方式共享 4.0 国际许可协议，转载请注明出处。