ROS 2 Control 使用指南：控制器、Executor 与硬件接口

从“发一条命令”到“电机真的按照命令运动”

刚接触 ROS 2 控制时，很容易把几个问题混在一起：我已经创建了一个发布者，为什么机器人没有动？controller_manager 是一个 PID 吗？把 SingleThreadedExecutor 改成 MultiThreadedExecutor，控制频率是不是就提高了？URDF 里已经写了关节，为什么还需要 hardware_interface？

这篇笔记沿着一条实际的数据路径来回答：上层任务给出目标 → 控制器处理目标 → 硬件接口发送命令、读取反馈 → 控制器根据新状态继续运行。 先用两个虚拟关节把路径接通，再讨论多线程、插件开发和真机接入。

你之前提到的两个名称，正确拼写分别是 SingleThreadedExecutor 和 MultiThreadedExecutor。这里的 Executor 是“ROS 回调执行器”，不要和执行电机运动的 actuator 混淆。

本文以 Ubuntu 22.04 + ROS 2 Humble + 对应发行版的 ros2_control / ros2_controllers 为学习基线，资料核对日期为 2026-09-06。Humble 的补丁版本仍有差异，实际复现时要记录安装包版本；不能拿 Rolling 的硬件插件代码直接覆盖 Humble 的写法。

配套的双关节 mock 示例与 Executor 对比程序保存在同一份可下载源码中：示例说明与文件清单。本文中的命令是供你在 ROS 开发环境执行的教程，不是已经在当前 Docker 内完成编译、运行或真机验收的报告。网站构建通过，也不能替代 ROS 集成测试。

如果只想先跑通一次，读“最小双关节实验”；如果你已经能运行例程，但不知道线程和接口为何这样设计，重点读“Executor”“硬件接口”和“实时边界”。通信策略另见 ROS2 QoS 配置策略。

一、先把几个名字分开

1.1 ros2_control、ros2_controllers 和 controller_manager

控制器和硬件通常以 pluginlib 插件形式装入同一个控制进程；控制器拥有 ROS 节点接口，但不代表每个控制器都要单独启动一个可执行进程。普通使用时，启动官方 controller_manager 包里的 ros2_control_node 即可，无须自己重写一个 manager。官方 Humble 架构说明。

1.2 一条命令到底经过哪些地方

你的任务节点 / MoveIt / 遥操作 / 测试脚本
                 │  Topic 或 Action
                 ▼
        控制器的 ROS 回调                 非实时侧
                 │  校验 + 线程间数据交换
                 ▼
 ┌──────── controller_manager 的更新路径 ────────┐
 │ read() → controller.update() → write()       │
 │   ▲          │                  │            │
 │ 状态接口     └── 写命令接口 ─────┘            │
 └───│─────────────────────────────│─────────────┘
     │                             ▼
     └── 设备反馈 ← 硬件插件/驱动 ← 设备命令
                    CAN / EtherCAT / 串口 / SDK

这张图里有两种完全不同的“接口”：

• ROS 通信接口，例如 /arm_controller/follow_joint_trajectory，跨节点传递目标。
• ros2_control 硬件接口，例如 joint1/position，在控制器与硬件插件之间传递一个具体物理量。

后者不是一个叫作 /joint1/position 的 ROS topic。不能通过 ros2 topic pub /joint1/position ... 直接访问这块硬件接口。接口的存在、可用性和占用情况，要用 ros2 control list_hardware_interfaces 检查。

因此，QoS 主要作用于图中进入、离开 ROS 通信边界的数据；不能给一个内存中的 command interface 配置 reliable()，也不能靠把 QoS 调成 Reliable 修复错误的电机单位。

1.3 三个频率，不要写成一个频率

假设上层每 50 ms 发一次目标，控制循环每 10 ms 更新一次，电机内部每 1 ms 执行一次伺服：

这些数字是说明层次的示例，不是任何机器人的统一推荐值。JTC 可以接收一段带时间的轨迹，在多个更新周期内插值；它不需要你每个控制周期重新发一次完整 Action。反过来，某些速度遥操作控制器需要持续收到新命令，超时就停止。输入语义不同，不能套用同一种“发一次以后一直保持”的假设。

二、最小双关节实验：先把路径接通

2.1 这个实验能证明什么

本例使用 mock_components/GenericSystem，不连接电机，不需要 RViz，也不需要 Gazebo。它把命令映射为理想化状态，适合检查 URDF、控制器参数、资源占用和轨迹接口。官方 Mock Components 说明。

它不能证明电机真的跟踪了目标，也不能证明动力学、摩擦、重力补偿、碰撞、制动或通讯延迟正确。看到 mock 的误差很小，往往是因为状态本来就是由命令生成的，不是你已经调出了一个非常好的伺服器。

本例故意保持简单：

• 关节：joint1、joint2，均为转动关节。
• 命令接口：每个关节一个 position。
• 状态接口：每个关节 position、velocity。
• joint_state_broadcaster 发布状态。
• arm_controller 使用 joint_trajectory_controller/JointTrajectoryController。
• forward_position_controller 仅预置配置，不默认加载，留给后面的接口切换实验。
• manager 名称为 /controller_manager；不启用仿真时钟。

2.2 源码目录与下载入口

lsqy_control_demo/
├── CMakeLists.txt
├── package.xml
├── README.md
├── config/
│   └── controllers.yaml
├── launch/
│   └── demo.launch.py
├── urdf/
│   └── two_joint_arm.urdf
└── src/
    └── executor_demo.cpp

直接查看：CMakeLists.txt、package.xml、URDF、控制器 YAML、launch、Executor C++ 源码。下载后应保持上述目录结构，而不是把所有文件放在一个平级目录。

也可以 一次下载完整源码 ZIP，并用 SHA-256 校验清单 检查文件是否完整。ZIP 的根目录就是 lsqy_control_demo，解压到自己的 colcon 工作空间 src/ 后，应得到 src/lsqy_control_demo/package.xml，不要再套一层同名目录。它只包含源码与文档，不含 ROS 依赖、编译结果或已验收的镜像。

如果使用网站仓库里的 ROS 开发容器，仓库挂载路径是 /workspace，原始包位于：

/workspace/source/downloads/code/ros2_control_humble/lsqy_control_demo

本文不更改现有 Dockerfile，也不声称当前 ROS 镜像已经装齐这些依赖。建议在 ROS 开发环境中做实验，网站本身仍在宿主机生成；不要为了运行这个例程给 macOS 的 Hexo 环境安装 ROS 原生依赖。

2.3 准备依赖和编译

以下命令以已经正确安装 ROS 2 Humble 软件源的 Ubuntu 22.04 环境为前提。安装依赖会修改该环境；不是让你在任意宿主机直接复制执行。

source /opt/ros/humble/setup.bash

sudo apt-get update
sudo apt-get install \
  ros-humble-ros2-control \
  ros-humble-ros2-controllers \
  ros-humble-robot-state-publisher \
  ros-humble-rclcpp \
  ros-humble-std-msgs \
  python3-colcon-common-extensions

ros2_control 和 ros2_controllers 的 Humble 二进制安装方式见 官方安装说明。软件源问题应先排查 ROS 安装环境，不要把“找不到包”归因于示例的 C++ 代码。

如果希望这些依赖在迁移后仍然存在，应把安装依赖的步骤明确加入 ROS 开发 Dockerfile 的 ROS 软件源配置之后、切换非 root 用户之前，再重新构建镜像；只在一个运行中容器执行 apt，不会自动更新 Dockerfile。配套 README 给出了待实施的 Dockerfile 片段，本文没有替你更改或重建当前镜像。

下面使用仓库挂载目录中的独立构建空间，源码仍留在原处，构建产物写入 /workspace/build/ros2-control-demo-ws。这与下载包 README 的主路径一致，宿主机绑定挂载可以保留构建日志；如果该目录已经用于其他实验，先换一个新目录，后续 source 路径也要同步替换。

启动之前先隔离教学实验：确认选择的 ROS domain 未被其他实验使用，并确保这个容器不连接实体机器人。下面的 77 只是示例，不是保留的教学域。所有控制与 CLI 终端都应通过 docker compose ... exec dev bash 进入同一个开发容器，使用相同环境变量；不要在一个容器启动、另一个容器的 localhost 发命令。这些设置减少意外连接，不是安全认证机制。

export ROS_DOMAIN_ID=77
export ROS_LOCALHOST_ONLY=1

mkdir -p /workspace/build/ros2-control-demo-ws
cd /workspace/build/ros2-control-demo-ws
colcon build \
  --base-paths /workspace/source/downloads/code/ros2_control_humble/lsqy_control_demo \
  --packages-select lsqy_control_demo \
  --symlink-install
source install/setup.bash
ros2 launch lsqy_control_demo demo.launch.py

若使用 ZIP 解压到自己的工作空间，则在该工作空间根目录执行 colcon build --symlink-install --packages-select lsqy_control_demo，无需上述 --base-paths。不要把容器未挂载的 home 当成永久保存位置；build/ 虽然可以保留在宿主机，也不表示它已被 Git 跟踪或已经备份。

另开终端时，不会继承前一个终端的 source：

source /opt/ros/humble/setup.bash
source /workspace/build/ros2-control-demo-ws/install/setup.bash
export ROS_DOMAIN_ID=77
export ROS_LOCALHOST_ONLY=1

2.4 URDF：结构和控制资源是两层描述

普通 URDF 的 <joint> 描述父子 link、旋转轴、关节类型和物理限位；<ros2_control> 里的 <joint> 描述控制框架要访问哪些变量。它们通过完全一致的关节名字关联，不是二选一。

本例的控制部分形如：

<ros2_control name="DemoSystem" type="system">
  <hardware>
    <plugin>mock_components/GenericSystem</plugin>
    <param name="calculate_dynamics">false</param>
  </hardware>
  <joint name="joint1">
    <command_interface name="position"/>
    <state_interface name="position">
      <param name="initial_value">0.0</param>
    </state_interface>
    <state_interface name="velocity">
      <param name="initial_value">0.0</param>
    </state_interface>
  </joint>
  <joint name="joint2">
    <command_interface name="position"/>
    <state_interface name="position">
      <param name="initial_value">0.0</param>
    </state_interface>
    <state_interface name="velocity">
      <param name="initial_value">0.0</param>
    </state_interface>
  </joint>
</ros2_control>

这是一段说明性摘录，完整机器人结构以下载的 URDF 为准，组件名为 DemoSystem。calculate_dynamics=false 表明不要求 mock 根据位置变化推导速度；本例又没有 velocity 命令接口，所以不要把里面的 velocity=0 当成真实测速结果。若后续打开有限差分或积分功能，它依然不是包含电机动力学的仿真。

尤其要避免一个误解：给 position 写一个 initial_value=0，只是初始化虚拟状态，不等于已经完成真实编码器的零位校准。

2.5 YAML：manager 参数与控制器参数不在同一层

最重要的配置关系如下，完整参数以 controllers.yaml 为准：

controller_manager:
  ros__parameters:
    update_rate: 100
    use_sim_time: false
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
    arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController
    forward_position_controller:
      type: forward_command_controller/ForwardCommandController

arm_controller:
  ros__parameters:
    joints: [joint1, joint2]
    command_interfaces: [position]
    state_interfaces: [position, velocity]
    allow_partial_joints_goal: false
    open_loop_control: false

forward_position_controller:
  ros__parameters:
    joints: [joint1, joint2]
    interface_name: position

理解两句话就能避免大量配置错误：

1. controller_manager.ros__parameters.arm_controller.type 告诉 manager：“名为 arm_controller 的实例，应该加载哪种插件。”
2. arm_controller.ros__parameters.joints 告诉这个控制器实例：“你具体要控制哪些关节。”

arm_controller 是可以自定义的实例名；joint_trajectory_controller/JointTrajectoryController 是插件类型名，不能随意改写。一个插件类型可以实例化多次，但多个实例能否同时激活，还取决于是否争抢同一个命令接口。

配置文件里出现 type 不等于插件已经被加载，出现 joints 也不等于控制器已经激活。必须看运行时状态。

下载配置还明确设置了几个容易混淆的参数：

这些是教学 mock 的配置，不是真机调参结论。容差是判断是否满足轨迹要求的条件，不是输出限幅器；把容差调大只会放宽成功标准，不会提高跟踪能力。参数细节应对照 Humble JTC 参数说明。

2.6 launch 做了什么，为什么不靠 sleep 等启动

本例启动三个角色：

• robot_state_publisher 读取 URDF，提供机器人描述并根据关节状态发布 TF。
• ros2_control_node 读取控制器参数，通过 ~/robot_description → /robot_description 的 remap 接收机器人描述。
• spawner 依次加载、配置、激活 joint_state_broadcaster 与 arm_controller；失败时 launch 结束实验，避免后台留下一套不完整的启动状态。

Humble 当前文档已将直接给 manager 传 robot_description 参数标为弃用，推荐使用机器人描述 topic。本例采用 topic 路径。较早的 Humble 安装如果表现不同，应先核对安装版本，不要同时混用多份不一致的机器人描述。Humble Controller Manager：描述输入与 spawner。

spawner 会等待 manager 的服务可用，然后按阶段完成加载。它正常退出不等于控制器消失：插件仍由 manager 持有。给 launch 塞一个固定 sleep 5，只能猜机器启动速度，不能证明服务、硬件或控制器已经 ready。

2.7 先检查资源，再发目标

另一个终端执行：

ros2 node list
ros2 control list_controllers -c /controller_manager
ros2 control list_hardware_components -c /controller_manager
ros2 control list_hardware_interfaces -c /controller_manager
ros2 action list -t

预期应满足以下条件，而不是逐字匹配某一版 CLI 的输出排版：

• joint_state_broadcaster 与 arm_controller 均为 active。
• mock 硬件组件可用且处于适合运行的状态。
• command 部分存在 joint1/position、joint2/position，激活 JTC 后已被占用。
• state 部分存在两个关节的 position、velocity。
• 存在 /arm_controller/follow_joint_trajectory，类型为 control_msgs/action/FollowJointTrajectory。

状态的实际名称与可选参数，可用 ros2 control <子命令> -h 查询。Humble CLI 文档。

active 只是生命周期条件，不是“目标已完成”的证据。接下来还要检查目标是否被接受、反馈是否变化和结果是否成功。

2.8 发送一个两秒轨迹

ros2 action send_goal \
  /arm_controller/follow_joint_trajectory \
  control_msgs/action/FollowJointTrajectory \
  "trajectory:
    joint_names: [joint1, joint2]
    points:
      - positions: [0.5, -0.3]
        time_from_start: {sec: 2, nanosec: 0}
  " \
  --feedback

这句话的准确含义是：从这段轨迹的起点计时，希望在第 2 秒让 joint1 到达 0.5 rad、joint2 到达 -0.3 rad。不是“先等两秒，再瞬间跳到这两个角度”。这里没有指定绝对开始时间，消息 header 默认零时间，表示尽快开始。

另开终端看状态：

ros2 topic echo /joint_states --once
ros2 topic list -t
ros2 topic info /joint_states -v

关节数组必须按消息里的 name 对齐，不要假定第三方广播器永远用某个固定顺序。Action 的反馈与结果要分别看：Goal accepted ≠ Result succeeded；还应检查最终状态为 SUCCEEDED、结果 error_code 为 0，并核对按关节名对应的实际位置。本例在理想 mock 下应该最终接近目标，但那是待你在 ROS 环境验证的预期行为，不是本文虚构的一段运行日志。

JTC 的 Action 接口允许检查执行结果与容差；topic 接口更接近“发送后不等待结果”。需要记录实验是否成功时，优先把 Action 的最终状态、错误码与误差记录下来。Humble JTC：命令与 Action 接口。

2.9 为什么只有 position 的轨迹还不够“平滑”

时间点只提供位置时，默认 spline 路径使用线性插值；增加一致的速度边界可以使用三次插值，再增加一致的加速度边界可使用五次插值。连续的阶次不同，机械系统感受到的冲击也不同。官方轨迹表示。

例如，在确认上一段已经完成，且当前接近 [0.5, -0.3] 后，可以用两个明确的位置点和零端点速度尝试返回：

ros2 action send_goal \
  /arm_controller/follow_joint_trajectory \
  control_msgs/action/FollowJointTrajectory \
  "trajectory:
    joint_names: [joint1, joint2]
    points:
      - positions: [0.5, -0.3]
        velocities: [0.0, 0.0]
        time_from_start: {sec: 0, nanosec: 0}
      - positions: [0.0, 0.0]
        velocities: [0.0, 0.0]
        time_from_start: {sec: 3, nanosec: 0}
  " \
  --feedback

这里只用于 mock 观察插值。如果真实机器人没有位于第一个点，t=0 的指定位置可能造成不连续目标。真实系统应以可靠反馈构建起点，并通过规划、速度/加速度限制与设备保护约束整段轨迹。加上 velocities: [0, 0] 不会自动让任意错误轨迹变得安全。

三、控制器怎么选，不要看到 Controller 就认为是 PID

3.1 常用控制器对照

joint_state_broadcaster 默认可以读取状态并发布 /joint_states，自定义状态量可通过 /dynamic_joint_states 观察；它不占用运动命令接口。官方 JSB 说明。

Forward 控制器的 ~/commands 使用 std_msgs/msg/Float64MultiArray，数组本身没有关节名字，也没有标准的命令时间戳。数组顺序由配置中的 joints 决定。给它发 [0.5, -0.3]，前提是你已经确认这个实例的关节顺序和接口类型。官方 Forward Command Controller。

3.2 位置控制器为什么不一定需要 ROS 里的 PID 参数

考虑两台不同的设备：

设备 A：接收目标角度，驱动器内部已有位置/速度/电流环
  JTC 插值位置 → position command → 驱动器内部闭环

设备 B：接收电流或力矩，ROS 侧需要计算合适的力矩目标
  位置参考 + 编码器反馈 → 控制律 → effort command → 驱动器

在 JTC 的 position 命令模式中，轨迹给出的目标位置主要转发给硬件；这不等于 JTC 为你在 ROS 层实现了一个位置 PID。采用 velocity-only 或 effort 接口时，需要按照该控制器支持的接口组合、状态输入和参数确定误差如何映射为命令。JTC 硬件接口与控制律。

因此，调参之前先回答：闭环到底在哪里闭合，哪个环的输入是什么，反馈来自哪里？ 如果设备只接受目标角度，给 YAML 随意添加一组 gains，不代表那组增益真的参与了电机闭环。

同样，open_loop_control 的含义是控制器如何选择用于推进控制的状态，不是一个“打开后更稳定”的通用选项。不要为了消除跟踪误差日志，把真实反馈从计算链里绕过去。

3.3 差速底盘的一个配置思路

差速底盘的关节命令通常是左右轮角速度，而上层使用机体前进速度和偏航角速度。两轮简化几何关系为：

左轮角速度 = (前进速度 - 偏航角速度 × 轮距 / 2) / 轮半径
右轮角速度 = (前进速度 + 偏航角速度 × 轮距 / 2) / 轮半径

要逐个核对符号方向：右手坐标系下的正偏航、URDF 关节轴方向、驱动器正转方向，不一定天然一致。几何参数错了，不应先用 PID 去抵消。

Humble 的 diff_drive_controller 使用 wheel velocity command；反馈可按配置选择轮位置或速度。输入可以是带时间戳的 ~/cmd_vel，也可以在对应配置下使用不带时间戳的 ~/cmd_vel_unstamped。它还提供命令超时处理。消息类型、话题名和 use_stamped_vel 必须配套，不能从别的发行版复制一个 /cmd_vel 名称就认定接口兼容。Humble Diff Drive Controller。

如果同时运行里程计融合节点，要明确谁发布 odom → base_link；两个节点同时发布同一 TF 边，会出现看似控制抖动、实则坐标冲突的问题。

四、controller_manager：生命周期、占用和切换

4.1 load、configure、activate 是三个动作

插件类型可被发现
    ↓ load
unconfigured：实例已建立，还没有完整运行配置
    ↓ configure
inactive：参数和非实时资源准备好了，但还没有参与正常控制
    ↓ activate / claim interfaces
active：按框架规则参与更新并使用所需接口
    ↓ deactivate / release interfaces
inactive

spawner 把几个阶段串起来，是便利工具，不是新的控制框架。排错时要识别在哪一步失败：

• load 失败：类型名不对、库没安装、pluginlib 没发现，或初始化失败。
• configure 失败：缺少参数、关节列表非法、依赖资源没有准备好。
• activate 失败：硬件接口不存在、不可用、已被别的控制器占用，或激活逻辑拒绝。
• active 后没有运动：再查命令输入、控制器更新、设备写入和真实反馈。

对于自定义控制器，所需 command/state 接口分别由 command_interface_configuration() 和 state_interface_configuration() 描述。update(time, period) 是控制计算入口；它与通过 get_node() 创建的订阅回调不是同一个概念。Humble ControllerInterfaceBase 定义。

4.2 同一个接口不能让两个控制器同时写

本例 arm_controller 使用：

joint1/position
joint2/position

forward_position_controller 也想写这两个接口，因此不能在两者都 active 时任由它们轮流覆盖目标。

状态接口的共享读取与命令接口的独占写入，是两回事。JTC 读取位置，JSB 也读取位置，是正常组合；两个位置控制器抢同一个位置命令接口，会发生资源冲突。

还有一个更隐蔽的情况：joint1/position 与 joint1/effort 是不同名字，框架按名字看不一定存在同一个资源的直接冲突。但一台真实驱动器可能不能同时工作在位置模式与力矩模式。名字不冲突，不代表硬件模式组合合法。 后面讲的 command mode switch 就是解决这一层契约。

4.3 用 mock 做一次可恢复的切换实验

以下命令只针对本例 mock。先让轨迹结束，再加载 Forward 控制器到 inactive：

ros2 run controller_manager spawner forward_position_controller \
  -c /controller_manager --inactive

ros2 control list_controllers -c /controller_manager

此时不要直接把第二个控制器激活来和 JTC 争抢。用一次切换请求停用原控制器、激活新控制器：

ros2 control switch_controllers \
  -c /controller_manager \
  --deactivate arm_controller \
  --activate forward_position_controller \
  --strict

ros2 control list_controllers -c /controller_manager
ros2 control list_hardware_interfaces -c /controller_manager

ros2 topic pub --once \
  /forward_position_controller/commands \
  std_msgs/msg/Float64MultiArray \
  "{data: [0.2, -0.1]}"

切回 JTC：

ros2 control switch_controllers \
  -c /controller_manager \
  --deactivate forward_position_controller \
  --activate arm_controller \
  --strict

ros2 control list_controllers -c /controller_manager

--strict 要求按严格策略判断切换成功；它不是“物理世界的事务回滚”。状态切换仍可能遇到插件错误或硬件拒绝，所以每次都要读取返回值和实际状态，再决定下一步。不要因为 service 返回成功就跳过目标连续性、制动和驱动器状态检查。Humble 切换服务定义、CLI 参数实现。

这个实验中的单次数组命令只是连通性检查。Forward 控制器不会替你生成从旧位置到新位置的三秒限速轨迹；真机不能把上述阶跃当作常规安全运动方式。

4.4 为什么“停掉 controller”不等于“电机已经安全停止”

控制器失活可能释放 command interface，但设备可能继续保持上一次命令，或者驱动器本身仍然使能。急停、制动器、驱动器 watchdog、总线掉线保护和供电状态，必须由明确的硬件/安全设计保证。

尤其是 Humble，不能假定 inactive 状态天然屏蔽所有运动写入。官方硬件生命周期文档明确保留了依赖硬件实现处理命令的边界。因此应在硬件层明确：inactive 时是否保持、停机、禁止运动或仅允许诊断操作，并实际测试。Humble 硬件生命周期。

五、Executor：单线程、多线程与回调组

5.1 Executor 执行的是哪类函数

订阅回调、timer 回调、service 请求处理、Action 相关回调，都需要某个 Executor 获得执行机会。创建 subscription 对象并不等于回调会自动运行；进程需要 spin() 或其他明确的执行安排。

典型单线程写法：

rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
executor.spin();

显式双线程写法：

rclcpp::executors::MultiThreadedExecutor executor(
  rclcpp::ExecutorOptions(), 2);
executor.add_node(node);
executor.spin();

普通 rclcpp::spin(node) 相当于使用一个单线程执行器。一个 Executor 可以管理多个节点；一个节点也可以包含多个回调组。node、callback、callback group、thread 不是一一对应关系。 ROS 2 Humble Executors 官方文档源码。

5.2 什么时候先选 SingleThreadedExecutor

单线程比较适合以下起点：

• 简单命令转发、轻量状态监测，每个回调都很短。
• 初期调试，希望减少共享变量并发读写带来的不确定性。
• 几个回调必须串行访问同一个不支持并发的对象。
• 节点本身没有需要同时处理的独立慢任务。

代价是，一个回调占用线程时，同一个执行器里的其他回调也要等待。例如诊断 timer 做了 700 ms 的阻塞 I/O，原本每 100 ms 执行一次的状态回调就可能严重延迟。

“使用单线程”也不等于整个进程只有一个线程。DDS、日志、驱动 SDK、你自己创建的工作线程以及控制循环，都可能另外创建线程。它只约束这个 Executor 如何调度它负责的回调。

5.3 什么时候 MultiThreadedExecutor 才真正有用

多线程适合存在相互独立且允许并行的回调，例如轻量指令接收与耗时诊断处理并存。前提是这些回调不会被同一个互斥回调组串行限制，而且共享资源访问已经设计好。

不要因为“控制器很多”就把线程数设成关节数。六关节 JTC 通常是在一次 update() 中处理六个关节；给 ROS Executor 六个线程，不会把六个关节自动变成六条实时控制任务。

线程数增加还可能增加上下文切换、锁竞争和内存压力。应先量测哪个回调阻塞了谁，再决定拆组、拆节点、移到后台工作线程或增加 Executor 线程，而不是把多线程当成通用性能开关。

5.4 Callback Group 才决定哪些回调允许并行

“允许并行”不是“必然同时执行”。还要有空闲线程、就绪事件和系统调度机会。回调组必须作为成员或其他长生命周期对象保留，不能创建一个临时 group 后丢弃引用。ROS 2 Humble Callback Groups 官方文档源码。

下面是类内创建方式的结构片段，完整可编译程序在下载包中：

// 成员：两个组与两个 timer 都需要保持存活。
rclcpp::CallbackGroup::SharedPtr fast_group_;
rclcpp::CallbackGroup::SharedPtr slow_group_;
rclcpp::TimerBase::SharedPtr fast_timer_;
rclcpp::TimerBase::SharedPtr slow_timer_;

// 构造函数内：不同互斥组，可以由多线程执行器并行调度。
fast_group_ = create_callback_group(
  rclcpp::CallbackGroupType::MutuallyExclusive);
slow_group_ = create_callback_group(
  rclcpp::CallbackGroupType::MutuallyExclusive);

fast_timer_ = create_wall_timer(
  std::chrono::milliseconds(100), fast_callback, fast_group_);
slow_timer_ = create_wall_timer(
  std::chrono::milliseconds(1000), slow_callback, slow_group_);

若是 subscription，则通过 rclcpp::SubscriptionOptions 的 callback_group 指定；service client、timer、Action 的创建 API 也有对应位置。不要只创建 group，却忘记把实体放入它。

5.5 一个能亲眼看出差异的小实验

配套 executor_demo 有两个 timer：一个每 100 ms 记录一次快回调；另一个每 1000 ms 触发，并人为等待 700 ms，模拟阻塞任务。这里的 sleep 只为了展示调度问题，不能复制进实时控制循环。

它是一个独立的普通 rclcpp 节点，不连接 mock 控制器；给它设置 executor:=multi，不会修改 /controller_manager 的线程配置，也不要求先启动 demo.launch.py。

编译并 source 示例工作空间后，每次只运行下面一种模式，观察十几秒，然后 Ctrl+C 停止，再运行下一种：

# A：一个执行线程，即使分了两个组，慢回调仍会阻塞快回调。
ros2 run lsqy_control_demo executor_demo \
  --ros-args -p executor:=single -p same_group:=false

# B：两个执行线程 + 两个互斥组，快回调有机会与慢回调并行。
ros2 run lsqy_control_demo executor_demo \
  --ros-args -p executor:=multi -p same_group:=false

# C：两个执行线程，但两个 timer 放回同一个互斥组，仍要串行。
ros2 run lsqy_control_demo executor_demo \
  --ros-args -p executor:=multi -p same_group:=true

你要比较的是快回调的实际间隔和执行线程，而不是日志能否精确对齐 100 ms。日志本身、普通操作系统调度和容器资源限制都会影响时间，所以这个实验不是实时性能基准，也没有在本文中预先宣布某个固定的抖动结果。

还有一个常见陷阱：timer 触发间隔短于回调执行时间时，换成 Reentrant 可能让同一回调多次执行重叠。多线程不是消除了过载，而可能把过载变成了并发重入问题。

5.6 服务调用的死锁：为什么服务端回复了，客户端仍卡住

考虑这个安排：timer 和 service client 位于同一个默认互斥组。

timer 回调开始，占用默认互斥组
    ↓
发出请求，然后等待 future.get()
    ↓
服务端已处理并回复
    ↓
客户端需要执行响应处理，但默认组仍被 timer 占用
    ↓
timer 等响应；响应等 timer 退出

即使外面使用 MultiThreadedExecutor，互斥组的约束仍然存在。换成多个线程，不会自动解除这个循环等待。

优先采用发送异步请求后立即返回，让结果回调继续处理下一步。下面是思路片段，client_ 和 request 已由调用方创建：

client_->async_send_request(
  request,
  [this](rclcpp::Client<std_srvs::srv::Trigger>::SharedFuture future) {
    const auto response = future.get();  // 结果回调触发时 future 已就绪
    RCLCPP_INFO(
      get_logger(), "request finished: %s",
      response->success ? "true" : "false");
  });
// 当前回调到这里返回，不在这里等待 future。

工程实现还需要服务可用性检查、超时、最多一个在途请求或明确的队列上限；超时请求应按对应 rclcpp API 清理 pending request。不要每 10 ms 无限制地创建一个永远等不到结果的请求。

将阻塞调用者与响应回调放到不同组、并保证有足够工作线程，有时能够避免这类死锁，但仍保留阻塞与资源耗尽风险。也不要为了等待响应，把已经加入某个执行器的同一个节点再次交给 spin_until_future_complete()。如果控制路径必须等待一个慢服务，就应重新思考状态机边界，而不是在周期循环内嵌套 spin。

六、最容易误解的地方：Executor 不等于控制循环

6.1 Humble 默认程序实际怎样运行

Humble 的 ros2_control_node.cpp 创建 MultiThreadedExecutor 处理 ROS 回调，并另外创建一个控制线程，在其中顺序调用 read()、update()、write()。这个结论来自对应 Humble 分支源码，而不是把其他 ROS 节点的 timer 模式套过来。Humble ros2_control_node 源码。

因此要分别看两条路径：

Executor 工作线程：接收轨迹 / 服务 / 参数等非周期控制回调
控制线程：        read → update 所有应更新的控制器 → write → 等待下一周期

这里是执行关系的简图，不是承诺所有生命周期转换都发生在非实时线程。尤其是激活、失活、接口切换会与控制周期协调，相关插件代码也应保持有界、避免不可控阻塞。

所以：

• ROS 回调慢，可能让新命令迟迟不能进入控制器。
• read() 慢，会拖延同一轮后续控制计算和写入。
• 某个控制器 update() 慢，可能拖延同一 manager 下其他控制器。
• write() 等设备回复太久，也会拖延整个控制周期。
• 给 Executor 增加线程，不能把默认同步 read/update/write 自动改造成并行硬件访问。

如果自己把 ControllerManager 嵌入一个自定义进程，线程安排可以重新设计，但那也意味着你要负责更新周期、退出顺序、接口切换、异常处理和实时约束。初学阶段先保留官方入口。

6.2 一个周期的时间预算

假设 manager 配置 100 Hz，名义周期是 10 ms。一次更新可以粗略拆成：

本周期所需时间 = T_read + T_controllers + T_write + T_framework + 调度抖动

如果设备读取需要 8 ms、算法需要 4 ms、写入需要 1 ms，光前三项就已经 13 ms。把参数写成 100 Hz，并不会让这条路径获得 10 ms 以内的魔法执行时间。

至少应测量：

• 实际周期的平均值、较高分位数与最大值。
• read/update/write 各段耗时。
• command age：本周期使用的目标有多旧。
• state age：本周期读取的反馈有多旧。
• 周期超时次数与持续时间，而不仅是平均频率。

ros2 topic hz /joint_states 测到的是一个观察者接收到状态消息的节奏，还受 broadcaster 配置、DDS、QoS 和观察节点调度影响；它不是控制循环的精确实时分析器。高频率也不等于低延迟，相关例子见 QoS 笔记。

6.3 命令从 ROS 回调进入 update，要有线程交换边界

错误的实现方式是：回调直接改一个 std::vector<double>，与此同时 update() 正在遍历这个 vector。即使订阅回调属于 MutuallyExclusive 组，也挡不住独立控制线程同时访问它。Callback Group 只约束 Executor 管理的回调，不会给任意 C++ 线程自动加锁。

一种常见结构是：

非实时回调
  类型、维度、有限值、关节名、时间戳检查
  ↓
写入预先设计的线程交换缓冲
  ↓
实时 update
  读取本周期可用的命令快照
  检查有效期与安全条件
  执行有界计算，写 command interfaces

realtime_tools::RealtimeBuffer<T> 是常见工具。Humble 实现的实时读取侧尝试获取锁，拿不到时保留已有数据；非实时写入侧可以等待。因此它不是“无锁队列”，也不保证每条中间命令都被消费。Humble RealtimeBuffer 实现。

选择 T 时也要考虑复制、析构与内存分配。较好的入门思路是固定尺寸命令结构，例如两个关节的目标数组、序号、接收时间与有效标志；在配置阶段初始化缓冲，不在每个 update 中不断创建、销毁大对象。缓冲器里放一个会在赋值时分配大块内存的对象，不会因为外面包了 RealtimeBuffer 就自动满足所有实时要求。

“只要最新目标”可以用最新值快照；“必须执行每一条轨迹/事务”需要明确的有界队列、接收确认与拒绝策略。两种语义不能互相替代。

6.4 不要从实时 update 直接做这些事

• 等待普通 service / Action 结果。
• 无超时地读串口、连 TCP、等待磁盘或网络。
• 高频打印日志，或者每周期写 CSV。
• 不断调整 vector 尺寸、创建线程、解析 YAML 或加载模型。
• 拿一个可能由慢线程长期持有的普通互斥锁。
• 用“消息能发布出去”代替发布路径的实时性分析。

需要发布状态时，可使用合适的 realtime_tools::RealtimePublisher 结构，把真正 ROS 发布工作交给非实时侧。实时侧优先使用有界的尝试获取方式并允许本次状态发布跳过，而不是等待发布者。消息中的可变长数组也应在非实时初始化阶段准备好。realtime_tools 官方说明。

这是设计原则，不表示每个使用 realtime_tools 的程序都已经通过硬实时验证。内核、调度优先级、内存、驱动、DDS 和完整调用链都必须在目标平台上测量。

七、硬件接口到底怎么定义

7.1 State Interface 与 Command Interface

对一个真实转动关节，可以先写出接口契约表，再动手写 C++：

直线关节对应的位置、速度、力一般使用 m、m/s、N。接口名相同，关节类型不同，物理单位也相应不同。设备协议里的 encoder tick、rpm、mA、degree，应由硬件适配层转换到约定单位，而不是让每一个上层控制器各猜一次。

SI 单位与右手坐标系的基本约定可查 REP-103 官方源码。如果某个领域必须使用不同约定，应明确记录并在接口边界转换，而不是保持同样的接口名却暗中改变单位。

状态值必须说明是测量、估计还是理想 mock。不能把“上一周期发出去的位置目标”伪装成“已经测到的真实位置”，否则跟踪误差会人为变小，掉线也可能被掩盖。

7.2 System、Actuator、Sensor 怎么选

重要的不是把每个电机都拆成一个类，而是选择正确的通信、故障和生命周期边界。例如总线要求一次同步读取全部电机，再一次同步写入全部目标，用一个 System 往往更容易保持一致快照。若设备真正独立，也可以设计多个组件。

这个选择会影响故障传播：一条总线组件失败，可能影响它管理的所有关节。把多个电机拆成多个对象，并不能改变它们实际共享同一条失效总线的事实。

7.3 Humble：接口句柄指向由插件持有的内存

Humble 中，常见实现通过 export_state_interfaces()、export_command_interfaces() 返回接口句柄，句柄内部指向插件持有的 double。控制器写 command 句柄，就是修改这块命令存储；write() 再把存储里的值变成设备协议。Humble handle 定义。

例如固定两个关节，用成员数组可以让地址保持稳定：

// 硬件插件的成员；必须比导出的接口活得更久。
std::array<double, 2> positions_{};
std::array<double, 2> velocities_{};
std::array<double, 2> position_commands_{};

状态导出的结构片段：

std::vector<hardware_interface::StateInterface>
TwoJointSystem::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> result;
  result.reserve(4);
  for (std::size_t i = 0; i < 2; ++i) {
    result.emplace_back(info_.joints[i].name, "position", &positions_[i]);
    result.emplace_back(info_.joints[i].name, "velocity", &velocities_[i]);
  }
  return result;
}

命令导出的结构片段：

std::vector<hardware_interface::CommandInterface>
TwoJointSystem::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> result;
  result.reserve(2);
  for (std::size_t i = 0; i < 2; ++i) {
    result.emplace_back(
      info_.joints[i].name, "position", &position_commands_[i]);
  }
  return result;
}

这里的 info_.joints 数量、名字和接口集合，必须先由 on_init() 校验；这段代码不能脱离检查直接拿来处理任意 URDF。函数里的 result 可以作为返回值移动出去，但它指向的 positions_ 不能是函数局部数组。

最危险的写法之一是导出 &vector[i] 后，运行中继续 push_back() 或 resize() 导致 vector 重分配；句柄仍拿着旧地址。另一种是回调线程和控制线程无同步地读写同一个 double：它不仅可能读到过时数据，还可能构成 C++ 数据竞争。

7.4 一个 Humble 硬件类需要哪些入口

下面是接口声明骨架，不是完整可编译驱动。它展示要实现的方法，设备 SDK、故障码、零点标定、I/O 超时等必须根据你的硬件补齐。可直接运行的教学路径仍然使用前面的 GenericSystem。

#include <array>
#include <string>
#include <vector>

#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/state.hpp"

namespace my_robot_hardware
{
class TwoJointSystem : public hardware_interface::SystemInterface
{
public:
  hardware_interface::CallbackReturn on_init(
    const hardware_interface::HardwareInfo & info) override;

  std::vector<hardware_interface::StateInterface>
  export_state_interfaces() override;

  std::vector<hardware_interface::CommandInterface>
  export_command_interfaces() override;

  hardware_interface::CallbackReturn on_configure(
    const rclcpp_lifecycle::State & previous_state) override;
  hardware_interface::CallbackReturn on_activate(
    const rclcpp_lifecycle::State & previous_state) override;
  hardware_interface::CallbackReturn on_deactivate(
    const rclcpp_lifecycle::State & previous_state) override;
  hardware_interface::CallbackReturn on_cleanup(
    const rclcpp_lifecycle::State & previous_state) override;
  hardware_interface::CallbackReturn on_shutdown(
    const rclcpp_lifecycle::State & previous_state) override;
  hardware_interface::CallbackReturn on_error(
    const rclcpp_lifecycle::State & previous_state) override;

  hardware_interface::return_type read(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;
  hardware_interface::return_type write(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

  hardware_interface::return_type prepare_command_mode_switch(
    const std::vector<std::string> & start_interfaces,
    const std::vector<std::string> & stop_interfaces) override;
  hardware_interface::return_type perform_command_mode_switch(
    const std::vector<std::string> & start_interfaces,
    const std::vector<std::string> & stop_interfaces) override;

private:
  std::array<double, 2> positions_{};
  std::array<double, 2> velocities_{};
  std::array<double, 2> position_commands_{};
  bool enabled_{false};
  // 还需要：设备驱动、关节映射、单位转换、限位、故障与时间状态。
};
}  // namespace my_robot_hardware

关键签名以 Humble SystemInterface 头文件 为准。CallbackReturn 用于生命周期回调，hardware_interface::return_type 用于读写及模式切换，不能因为都出现 ERROR 就把类型混着返回。

7.5 每个生命周期阶段应该做什么

激活时，把 position command 初始化为刚刚确认有效的当前位置，通常比无条件写零更合理，但它也不自动解决刹车释放、重力负载和驱动模式切换。速度/力矩模式同样不能照搬位置模式的初始化方式：零力矩可能让垂直机械臂下坠，而不是安全保持。

返回 ERROR 也不是“框架已经帮我完整安全恢复”的同义词。Humble 文档对硬件错误后自动协调控制器重启仍保留未完整实现的边界；故障时先依赖实际实现的设备保护和外部安全路径，再检查组件/控制器状态与恢复步骤，不能假定重新收到命令就会安全复位。Humble Controller Manager：重启硬件的限制。

7.6 read / write 里的核心转换

先把关节侧与电机侧约定写清楚。一个简化的转动关节换算可以是：

关节位置 rad = 方向符号 × (编码器计数 - 零点计数)
                × (2π / 每电机转计数) / 减速比

电机目标计数 = 零点计数 + 方向符号 × 关节目标 rad
                × 减速比 × 每电机转计数 / (2π)

这是用于理解的单圈线性映射。实际还可能涉及多圈编码、溢出、非整数传动、关节耦合和不同采样时间，不能用一个公式覆盖所有驱动器。

read() 应分辨“总线收到报文”与“得到本周期有效反馈”。至少考虑报文完整性、序号、设备故障、数据有限性、反馈时间和异常跳变。速度如果由差分估计，要使用合适的实际时间间隔，并说明滤波带来的延迟。

write() 应分辨“控制器提出目标”与“目标允许发送”。至少考虑当前使能/模式、目标是否有限、关节限位、速度/变化率、通信状态和 watchdog。读取到 NaN、错误单位或突变目标时，安全策略应明确拒绝、限幅或故障停机，而不是交给整数类型转换后随机发送。

当 SDK 只能阻塞读取时，可以评估独立 I/O 线程与有界双缓冲快照，但代价是状态年龄和写入延迟变成新的控制因素。把阻塞操作移到线程里，不代表反馈变得即时；应把“最近一次成功接收时间”和“本周期使用的数据年龄”纳入验证。

7.7 模式切换：接口能加载，不代表模式能同时工作

如果硬件支持 position 与 effort 两种模式，应在 prepare_command_mode_switch() 中检查即将启停的接口组合是否符合设备能力，提前准备转换资源；在 perform_command_mode_switch() 中完成适合控制周期执行的、时间有界的切换动作。

模式切换请求会包含与其他组件有关的接口，因此实现必须只处理自己拥有的接口；不要因为看到陌生关节名，就把整个机器人的切换请求拒绝。更不能对不属于自己的电机发送停机或模式命令。

切换应明确目标衔接：位置模式的当前位置、力矩模式的初始输出、积分器状态、驱动器状态字、制动条件和切换失败后的行为。框架能协调接口所有权，不能替你推导“从大力矩输出直接切到零位置目标”在物理上是否可接受。

八、pluginlib：URDF、C++、XML、CMake、package.xml 如何对应

8.1 不是只有一个 C++ 类就够了

假设未来新增一个独立的硬件包 my_robot_hardware，其类是 my_robot_hardware::TwoJointSystem。这不是当前下载的 lsqy_control_demo 中已经实现的真机驱动，而是解释插件工程结构的命名示例。

URDF 的 <plugin>my_robot_hardware/TwoJointSystem</plugin>
          ↓ 按插件名寻找
plugin XML 的 class name="my_robot_hardware/TwoJointSystem"
          ↓ 指定实际 C++ 类型
class type="my_robot_hardware::TwoJointSystem"
          ↓ 从指定共享库加载
library path="my_robot_hardware"
          ↓ 库已由 CMake 编译、安装，并进入 ament 索引
PLUGINLIB_EXPORT_CLASS(具体类型, hardware_interface::SystemInterface)

查找名称、C++ 名称和库目标名称可以不同，但必须对应一致。Humble 硬件插件编写说明。

8.2 插件注册与 XML

在硬件实现 .cpp 的命名空间外导出类：

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(
  my_robot_hardware::TwoJointSystem,
  hardware_interface::SystemInterface)

my_robot_hardware.xml：

<library path="my_robot_hardware">
  <class
    name="my_robot_hardware/TwoJointSystem"
    type="my_robot_hardware::TwoJointSystem"
    base_class_type="hardware_interface::SystemInterface">
    <description>Two-joint hardware system adapter.</description>
  </class>
</library>

机器人 URDF 的硬件块再指定：

<hardware>
  <plugin>my_robot_hardware/TwoJointSystem</plugin>
  <param name="device">/dev/robot_bus</param>
</hardware>

device 是你自己的硬件参数，不是 ros2_control 内置的万能驱动选项。必须由 on_init() 解析并校验。URDF 不应该存口令、访问令牌或其他私密凭据。

8.3 CMake 的最小关系

以下 CMake 片段属于将来完整实现的硬件包；前面的声明骨架还没有 .cpp 方法实现，不能拿它直接宣布构建完成。

cmake_minimum_required(VERSION 3.8)
project(my_robot_hardware)

find_package(ament_cmake REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)

add_library(my_robot_hardware SHARED src/two_joint_system.cpp)
target_compile_features(my_robot_hardware PUBLIC cxx_std_17)
target_include_directories(my_robot_hardware PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
ament_target_dependencies(my_robot_hardware
  hardware_interface pluginlib rclcpp rclcpp_lifecycle)

pluginlib_export_plugin_description_file(
  hardware_interface my_robot_hardware.xml)

install(TARGETS my_robot_hardware
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)
install(DIRECTORY include/ DESTINATION include)

ament_export_include_directories(include)
ament_export_libraries(my_robot_hardware)
ament_export_dependencies(
  hardware_interface pluginlib rclcpp rclcpp_lifecycle)
ament_package()

package.xml 至少要声明相应的依赖和构建类型，例如：

<buildtool_depend>ament_cmake</buildtool_depend>
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<export>
  <build_type>ament_cmake</build_type>
</export>

完整 package.xml 还需要包名、版本、描述、维护者与许可证等元数据。设备 SDK 依赖、安装路径、运行库搜索路径也必须纳入工程；“我的终端里能找到 .so”不等于迁移到另一台机器仍能找到。

8.4 插件加载失败时按这条顺序查

ros2 pkg prefix my_robot_hardware
ros2 pkg prefix hardware_interface
ros2 control list_controller_types -c /controller_manager

最后一条列出的是控制器类型，不是硬件插件列表；不要因为某硬件类没有出现在那里，就判断它注册失败。硬件是否已加载，应看 manager 启动日志和 list_hardware_components。

依次核对：

1. 终端 source 的是不是刚编译的 install 空间。
2. URDF 使用的插件查找名是否与 XML 的 name 一致。
3. XML 的 C++ type、base_class_type 是否与注册宏一致。
4. 共享库是否实际安装，依赖库是否存在，CPU 架构是否匹配。
5. CMake 是否导出了插件描述文件并调用 ament_package()。
6. 是否混入另一个发行版或另一个 overlay 中的同名包。

不要先把所有路径写进 LD_LIBRARY_PATH 来掩盖索引和安装错误。那往往会让当前终端碰巧成功，却让 Docker、launch 或另一台机器再次失败。

九、如果真的要写一个自定义控制器

9.1 不要从直接操作硬件句柄开始

先定义四件事：输入是什么、需要哪些状态、输出是什么、什么时候拒绝输入。比如一个关节 PD 示例的契约是：

参考：目标位置 q_ref
反馈：关节位置 q、关节速度 dq
输出：关节力矩 tau
控制律：tau = kp × (q_ref - q) - kd × dq

这只是教学公式，不含重力补偿、摩擦、耦合、饱和、关节软限位和输出变化率限制。它需要 effort command，不能直接放到本例只提供 position command 的 mock 配置中，就假装同一个控制系统已经变成力矩控制。

自定义类通常继承 controller_interface::ControllerInterface，通过插件导出；声明所需接口，配置非实时资源，在 on_activate() 验证可用接口并建立安全初始条件，在 update() 做有界计算。

9.2 先按名字建立确定映射

接口声明的结构可以是：

controller_interface::InterfaceConfiguration
MyEffortController::command_interface_configuration() const
{
  return {
    controller_interface::interface_configuration_type::INDIVIDUAL,
    {"joint1/effort"}};
}

controller_interface::InterfaceConfiguration
MyEffortController::state_interface_configuration() const
{
  return {
    controller_interface::interface_configuration_type::INDIVIDUAL,
    {"joint1/position", "joint1/velocity"}};
}

如果实际项目通过参数决定关节列表，应在配置阶段解析和校验，不能把 joint1 硬编码扩散到所有文件。也不要对 ALL 方式得到的接口数组顺序做未经验证的假设；建立按名字的确定映射，能避免“控制 joint1 却读了 joint2 的速度”这种危险错误。

9.3 update 之外同样需要设计

即使控制律只有一行，完整控制器还需要：

• 输入有限值、维度、关节名和时间的校验。
• 尚未收到目标时的行为。
• 上层掉线、目标过期和重连时的行为。
• 命令饱和与积分器处理；无积分项的 PD 也需要输出限制。
• 激活/失活的目标初始化，避免命令跳变。
• 只在非实时侧变更参数，或使用经过设计的参数快照交换。
• 状态发布与故障诊断，不阻塞控制周期。
• 每个接口的单位、坐标约定、顺序和资源占用测试。

可以阅读上游 Forward 控制器如何把订阅收到的数据放入 realtime buffer，再在 update() 中使用，但不要把它当成具备所有真机保护的万能模板。Humble Forward 控制器实现。

十、Humble 与新版本的边界

搜索教程时最容易踩的坑，是标题只写“ROS 2”，代码却混用了不同发行版的 API。

Jazzy 当前的硬件编写文档已经介绍框架管理节点、新的初始化上下文与自动接口管理方式，这些不是本文 Humble 手动导出例子的直接替代品。Jazzy 硬件接口说明，用于版本对照。

Rolling 还会继续发展，不能把“文档里能搜到”当成“当前 apt 安装的 Humble 就有”。遇到编译错误，先核对安装头文件和版本，再判断是不是自己 C++ 写错；源码分支也要记录 commit，单独写 humble 只固定了分支名，没有冻结内容。

十一、常见故障：按数据流排查

有三种“状态”尤其容易混淆：controller 的 lifecycle state、硬件组件的 lifecycle state、机器人真实物理状态。它们相关，但不能互相替代。

十二、从 mock 迁移到实机的检查清单

12.1 先确认软件链，再逐级引入真实因素

建议按下面的顺序积累证据：

1. 静态契约：URDF 可解析，YAML 层级正确，接口名和单位一致，插件能发现。
2. mock 链路：控制器正常激活，Action 接受与完成，状态变化符合目标，接口冲突可解释。
3. 更真实的仿真：引入动力学、执行延迟、噪声和限幅，观察误差与控制周期。
4. 设备通信但禁止运动：验证编码器、故障码、方向、零位、反馈时间、断连行为。
5. 受限实机运动：在符合设备要求的低风险条件下，验证小幅目标、停机、限位和掉线保护。
6. 目标平台与负载验收：在将来实际使用的系统、架构、内核和资源限制下测量长期运行表现。

每一级通过，只证明该级列出的条件。mock 成功不能跳过实机通信；Docker 能迁移不能跳过 CPU 架构、设备驱动与内核差异；一次目标到达不能替代超时和故障恢复测试。

12.2 需要逐项回答的实机问题

• 编码器的零点、正方向、减速比与关节轴是否已经验证？
• 驱动器接收 position / velocity / effort 的哪一种，闭环在哪里？
• read() 返回的数据最大允许多旧，连续丢几帧进入故障？
• 上层命令停止后，保持、制动还是受控停止？速度和力矩模式是否分别设计？
• command interface 的 min/max 参数最终由谁执行？不能只凭 URDF 里写了数值就断言已强制限幅。
• 初次使能、失活再激活、控制器切换、SDK 重连时，目标是否连续且符合设备安全要求？
• 返回 ERROR 以后，当前 Humble 版本和插件实际进入什么状态，需要人工恢复还是可以重新配置？
• 急停是否独立于 ROS 回调、网络连接和用户程序运行？
• 控制周期超时怎样被检测，故障日志是否足够定位到 read / update / write？
• 关节达到限位、目标出现 NaN、设备报故障或进程崩溃时，是否已经有独立测试记录？

容器部署时，还要明确串口/CAN/网卡映射、设备权限和实时调度权限。不要因为追求实时就默认使用 --privileged，也不要认为 macOS Docker Desktop 中的 Linux 容器等于原生 Linux 实时内核。 权限配置只是允许某些操作，不是实时性能已经成立的证据。

十三、把每次复现写成一份可比较的记录

“我运行成功了”信息量太少。建议每次实验至少记录：

实验目的：双关节 JTC 位置轨迹 / Executor 对比 / 硬件只读验证
日期：
宿主机系统、CPU 架构：
Docker 镜像名称与完整 Image ID（如果使用容器）：
ROS_DISTRO、RMW_IMPLEMENTATION、ROS_DOMAIN_ID：
ros2_control / ros2_controllers / rclcpp 的实际版本：
工作空间源码 commit 与未提交差异：
URDF、controllers.yaml、launch 文件：
硬件插件：GenericSystem / 真实插件与版本
update_rate、use_sim_time：
输入：完整 Action goal 或命令消息，包含时间：
判据：controller active、接口 claim、目标容差、超时策略：
结果：Action 最终状态与错误码、实际反馈、周期与数据年龄：
异常与复现步骤：
本次没有验证的边界：

Ubuntu 环境可以记录包版本：

printenv ROS_DISTRO RMW_IMPLEMENTATION ROS_DOMAIN_ID
dpkg-query -W 'ros-humble-controller-manager' \
  'ros-humble-hardware-interface' \
  'ros-humble-joint-trajectory-controller' \
  'ros-humble-rclcpp'
ros2 param get /controller_manager update_rate
ros2 param get /controller_manager use_sim_time
ros2 control list_controllers -c /controller_manager
ros2 control list_hardware_interfaces -c /controller_manager

环境变量未设置时，可能由默认实现或配置补充；空输出不代表没有 DDS 或没有 domain。若用了源码 overlay，apt 包版本也不完全代表当前实际加载的库，因此还应记录 ros2 pkg prefix 与源码版本。

十四、建议亲手做的六个练习

1. 只改关节名：在自己的实验副本中，把 YAML 一个关节名改错，观察失败发生在 configure 还是 activate，恢复后解释原因。
2. 接口冲突：保留 JTC active，再尝试直接激活 Forward，记录资源冲突；随后用正确切换请求恢复。仅在 mock 做。
3. 线程与回调组：运行 Executor 三种模式，对比快回调间隔；解释为什么“多线程 + 同一个互斥组”没有解决阻塞。
4. 通信与执行分层：记录 Action 接受时间、反馈变化时间、最终结果时间，解释为什么 Reliable QoS 不等于机械运动已经完成。
5. 反馈真实性：对比 mock 的 position 和 velocity，解释为什么“位置在变化、速度一直零”在这个配置里并不能证明真实速度为零。
6. 写接口契约：选一个自己熟悉的电机，只写出 command/state 单位、方向、零位、模式、超时与安全状态，不急着写 SDK 调用。

完成这些练习后，再去接一个真实硬件插件，通常会比一开始同时调 URDF、总线、PID、DDS 和线程更容易定位问题。

最后记住这几句话

• controller_manager 管控制器与接口资源，不等于某个固定控制算法。
• SingleThreadedExecutor / MultiThreadedExecutor 调度 ROS 回调；callback group 决定并发约束，独立控制线程不受它自动保护。
• 状态接口描述“现在怎样”，命令接口描述“希望怎样”；命令成功发送不是真实反馈。
• 硬件接口是一份关于单位、时间、内存、模式和安全行为的契约，不只是几行 export_*_interfaces()。
• mock 用于验证连接和接口；仿真用于验证部分动态行为；实机需要独立的控制与安全验收。
• 记录具体版本和验证边界，未来才有可能在统一的开发环境中真正复现问题。