第 6 章 驱动开发与运动控制¶
ros2_control 把硬件抽象为 interface、把控制算法抽象为 controller、用 controller_manager 统一调度二者的数据交换与执行周期。这一分层让同一套控制器代码可在仿真与真机之间无缝切换——只需替换 hardware_interface 的实现。本章从框架结构讲起，经自写 hardware_interface 与常用控制器配置，到实时性约束的落地要点，覆盖运动控制链路的完整开发路径。
1. ros2_control 框架与接口¶
1.1 四组件结构¶
| 组件 | 职责 | 典型类 |
|---|---|---|
| controller_manager | 调度器——按 update_rate 周期依次调用 read → update → write | controller_manager::ControllerManager |
| hardware_interface | 硬件抽象——read() 从设备读状态，write() 向设备写命令 | SystemInterface, ActuatorInterface |
| controller | 控制算法——在 update() 中计算命令值 | JointTrajectoryController, DiffDriveController |
| broadcaster | 状态发布——将内部状态转为 ROS 话题（如 /joint_states） |
JointStateBroadcaster, ImuSensorBroadcaster |
数据流的周期执行：
flowchart LR
hw[("硬件 / 总线")] -->|"read()"| hi["hardware_interface"]
hi --> cm["controller_manager<br/>按 update_rate 周期"]
cm -->|"update()"| ctl["active controllers"]
ctl -->|"新命令"| hi
hi -->|"write()"| hw
hi -.->|"状态"| bc["broadcaster"]
bc -->|"/joint_states、/odom"| top["ROS 话题"]controller_manager¶
调度器是一个 ROS 2 节点（ros2_control_node 的主体），持有 ResourceManager 与已加载的控制器集合。每个周期按固定顺序做三件事：对全部硬件组件调用 read()，按依赖顺序调用控制器的 update()，最后调用 write()。先读、再算、后写，使同一周期内所有控制器看到的是同一时刻的状态。
它同时是控制器生命周期的入口服务端：加载、配置、激活、切换都经它完成，ros2 control 子命令只是这些服务的命令行包装（「启动流程」）。资源分配也落在这一层——控制器激活时向 ResourceManager 申请接口，命令接口独占、状态接口可共享，申请不到即报 resource conflict 并拒绝激活（「接口类型」）。update_rate 决定它的调度周期，也是整条链路的时间基准。
hardware_interface¶
硬件组件是框架里唯一接触设备的实体，read() 与 write() 是它与总线之间的全部接口。基类按设备形态选：SystemInterface 把多个关节作为一个系统、ActuatorInterface 对应单个执行器、SensorInterface 只读、不声明命令接口。
组件自身也走 lifecycle：on_init 解析 URDF 中 <hardware> 的参数，on_configure 打开设备，进入 active 之后 read() / write() 才被周期调用。导出的接口是成员变量地址——export_state_interfaces() 交出指针，控制器与硬件因此读写同一块内存，每周期没有拷贝也没有序列化。
两个方法返回 return_type::OK / ERROR，总线故障用返回值上报而不抛异常：实时回路里异常与栈展开的耗时不可控。自写实现与串口 / CAN 细节见「最小 C++ 实现骨架」与「串口与 SocketCAN 通信」。
controller¶
控制器是控制算法的载体，统一实现 ControllerInterface：on_configure 读参数、声明所需接口，on_activate 完成运行前准备，update() 每周期从状态接口取值、把命令写回命令接口。
接口是静态声明的：command_interface_configuration() 与 state_interface_configuration() 在配置阶段列出所需接口名，运行期按 handle 访问，不再做名字查找；声明与实际不符时激活被拒（「接口类型」）。
控制器可以创建订阅者，但回调只把最新值放进实时缓冲，update() 从缓冲读取，实时线程因此不必与回调线程抢锁（「运行期禁分配」）。控制器之间还可以串联：上游的输出作为下游的参考输入，例如轨迹控制器给出位置目标、pid 控制器把它算成力矩。
broadcaster¶
broadcaster 同样实现 ControllerInterface，区别在于不申请命令接口：只读状态接口，把状态整理成话题发布。joint_state_broadcaster 汇总全部关节的 position / velocity / effort 发到 /joint_states，imu_sensor_broadcaster 把 IMU 的状态接口发布为 sensor_msgs/msg/Imu。
单独设这一层，是为了让「状态对外可见」与「控制回路」互不牵连：broadcaster 未激活时控制器照常运行，只是外部看不到状态；反过来增加或去掉一个观察者也不改变控制结果。
/joint_states 是 robot_state_publisher 的输入，非固定关节的 TF 由它算出，RViz 里的机器人模型与下游节点都依赖这条链——模型贴在初始位姿不动时，先查 broadcaster 是否被加载激活，再查 TF（第 5 章「TF2 与机器人建模」）。
1.2 URDF 中的 <ros2_control> 标签¶
ros2_control 的硬件配置直接写在 URDF/xacro 内，与运动学模型同一文件：
<ros2_control name="MyMotorSystem" type="system">
<hardware>
<!-- 指定 plugin 名称（编译后的 .so 文件名） -->
<plugin>my_pkg/MyHardwareInterface</plugin>
<param name="serial_port">/dev/ttyUSB0</param>
<param name="baudrate">115200</param>
</hardware>
<!-- 声明关节及其接口 -->
<joint name="wheel_left">
<command_interface name="velocity">
<param name="min">-2.0</param>
<param name="max">2.0</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="wheel_right">
<command_interface name="velocity">
<param name="min">-2.0</param>
<param name="max">2.0</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
1.3 控制器配置 YAML¶
controller_manager 从 YAML 文件加载控制器列表与参数：
controller_manager:
ros__parameters:
update_rate: 100 # Hz，控制周期 10ms
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
diff_drive_controller:
type: diff_drive_controller/DiffDriveController
diff_drive_controller:
ros__parameters:
left_wheel_names: ["wheel_left"]
right_wheel_names: ["wheel_right"]
wheel_separation: 0.30 # 轮距 (m)
wheel_radius: 0.05 # 轮半径 (m)
publish_rate: 50.0 # /odom 发布频率
odom_frame_id: odom
base_frame_id: base_link
pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
配置里四个关键项的含义：
update_rate是 controller_manager 的调度周期，决定 read→update→write 的频率。- 轮距必须与机械一致，否则转向角出现系统性偏差。
- 轮半径同理，偏差直接体现在直线速度上。
- 协方差对角线决定下游 EKF 对里程计的信任度。
1.4 接口类型¶
每个关节声明三种命令/状态接口中的一种或多种：
| 接口 | 含义 | 典型应用 |
|---|---|---|
position |
目标位置（rad / m） | 关节空间轨迹跟随 |
velocity |
目标速度（rad/s / m/s） | 差速底盘、连续旋转关节 |
effort |
目标力矩（N·m / N） | 力控、阻抗控制、仿真直驱 |
controller_manager 在启动时检查控制器请求的接口是否在 hardware 中可用——接口不匹配会报 resource conflict 并拒绝激活。
判据：先核对接口再激活
controller 请求的 command / state 接口必须与 hardware 导出的逐项对上；对不上时激活被拒，而 list_controllers 只报状态、不给原因。
2. 自写 hardware_interface¶
hardware_interface 是与物理设备交互的唯一入口。继承 hardware_interface::SystemInterface，实现 on_init、read、write 三个核心方法。
2.1 最小 C++ 实现骨架¶
完整骨架：继承 SystemInterface（点击展开）
##include "hardware_interface/system_interface.hpp"
##include "hardware_interface/handle.hpp"
##include "rclcpp/macros.hpp"
##include "rclcpp_lifecycle/state.hpp"
class MyWheelHardware : public hardware_interface::SystemInterface {
public:
// 从 URDF <hardware> 读取参数、初始化串口/CAN
hardware_interface::CallbackReturn on_init(
const hardware_interface::HardwareInfo& info) override {
// info.hardware_parameters.at("serial_port") → "/dev/ttyUSB0"
return hardware_interface::CallbackReturn::SUCCESS;
}
// 声明本硬件提供的接口
std::vector<hardware_interface::StateInterface> export_state_interfaces() override {
std::vector<hardware_interface::StateInterface> interfaces;
interfaces.emplace_back("joint1", "position", &pos_);
interfaces.emplace_back("joint1", "velocity", &vel_);
return interfaces;
}
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override {
std::vector<hardware_interface::CommandInterface> interfaces;
interfaces.emplace_back("joint1", "velocity", &cmd_vel_);
return interfaces;
}
// 每个周期读一次硬件——Humble 的签名带时间与周期参数（return_type 即 CallbackReturn）
hardware_interface::return_type read(
const rclcpp::Time& time, const rclcpp::Duration& period) override {
// 从串口/CAN 读取编码器 → pos_, vel_
return hardware_interface::return_type::OK;
}
// 每个周期写一次硬件
hardware_interface::return_type write(
const rclcpp::Time& time, const rclcpp::Duration& period) override {
// 将 cmd_vel_ 通过串口/CAN 发送给电机驱动器
return hardware_interface::return_type::OK;
}
private:
double pos_ = 0.0, vel_ = 0.0, cmd_vel_ = 0.0;
};
// 注册 plugin——编译为 .so 后由 <plugin> 标签引用
##include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(MyWheelHardware, hardware_interface::SystemInterface)
接口签名（Humble 的 system_interface.hpp 中 read(const rclcpp::Time&, const rclcpp::Duration&) 为纯虚函数）；跨版本升级时以目标分支头文件为准。
2.2 串口与 SocketCAN 通信¶
典型硬件交互模式——通过 /dev/ttyUSB* 串口与电机板通信，或通过 SocketCAN（can0）与 CANopen 伺服通信：
// SocketCAN 发送速度命令
##include <linux/can.h>
##include <linux/can/raw.h>
int sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
struct sockaddr_can addr = {.can_family = AF_CAN, .can_ifindex = if_nametoindex("can0")};
bind(sock, (struct sockaddr*)&addr, sizeof(addr));
struct can_frame frame = {};
frame.can_id = (0x200 + node_id) | (3 << 8); // CANopen RPDO2 典型映射：功能码 3 + 字节偏移 0（速度对象）
frame.can_dlc = 4;
int32_t vel_raw = static_cast<int32_t>(cmd_vel_ * gear_ratio * 1000);
memcpy(frame.data, &vel_raw, 4);
write(sock, &frame, sizeof(frame));
COB-ID 的功能码与字节偏移随设备的 PDO 映射配置（对象字典 0x1400 系）而变，上式为典型默认值——逐设备映射以 CANopen 词典为准。
CANopen 对象字典、SDO 配置与 PDO 映射的细节见 fieldbus-note 第 5 章。ros2_control 的 hardware_interface 只关心"在 write() 里把命令值发出去，在 read() 里把状态读回来"——总线协议封装在这一层内部，controller 不感知。
2.3 MockHardware 用于仿真¶
ros2_control 内置 mock HardwareInterface——所有命令直接赋给状态，无需真实硬件即可端到端运行：
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
仿真环境中（Gazebo、MuJoCo）用专用 hardware_interface 插件（gz_ros2_control、mujoco_ros2_control）替代 mock，实现物理引擎与控制器的闭环。
3. 控制器与运动控制¶
3.1 启动流程¶
controller_manager 启动后通过 lifecycle service 加载与激活控制器。两种等价做法——CLI 直接操作，或 launch 里交给 spawner：
## 加载并直接激活一个控制器
ros2 control load_controller --set-state active diff_drive_controller
## 查看当前 controller 状态
ros2 control list_controllers
from launch_ros.actions import Node
spawner_diff = Node(
package='controller_manager',
executable='spawner',
arguments=['diff_drive_controller'],
output='screen',
)
从 launch 到控制器 active，再到每个周期的读—算—写：
sequenceDiagram
participant L as launch
participant S as spawner
participant CM as controller_manager
participant C as controller
participant HW as hardware_interface
L->>S: 启动 spawner（控制器名）
S->>CM: load_controller(name)
CM->>C: 构造并 configure
S->>CM: switch_controller(activate)
CM->>C: activate
S-->>L: 退出（成功或超时报错）
loop 每个 update_rate 周期
CM->>HW: read()
HW-->>CM: 关节状态
CM->>C: update()
C-->>CM: 命令值
CM->>HW: write()
end3.2 差速底盘控制¶
diff_drive_controller 订阅 ~/cmd_vel，将线速度与角速度解算为左右轮目标转速，下发给 hardware_interface 的 velocity command interface。Humble 中该话题默认是 geometry_msgs/msg/TwistStamped（参数 use_stamped_vel 默认为 true）；设为 false 才退回 geometry_msgs/msg/Twist。
控制器自身根据左右轮实际编码器读数计算里程计，发布 ~/odom，并在 /tf 中发布 odom → base_link。它是控制器，不是独立 broadcaster。
接口：
- 输入：~/cmd_vel（TwistStamped，由导航或遥控发布）。
- 输出：~/odom（nav_msgs/Odometry）；/tf 中 odom → base_link 变换。
调参要点：wheel_separation 和 wheel_radius 必须与实际机械参数一致——偏差导致 转向角度与直线速度的系统性误差。pose_covariance_diagonal 与 twist_covariance_diagonal 决定里程计协方差，影响下游 EKF（robot_localization）对里程计的信任度。
坑：两个默认值
一是 use_stamped_vel 默认 true：发 geometry_msgs/msg/Twist 会不匹配，要么发 TwistStamped，要么显式设为 false。二是轮距/轮半径与机械不符时控制器不会报错，只表现为“直线跑偏、转角不准”。
3.3 关节轨迹控制¶
joint_trajectory_controller 接受 control_msgs/action/FollowJointTrajectory 目标，按时间参数化插值各关节位置/速度/加速度，逐周期下发 command。
## 发送轨迹目标（命令行）
ros2 action send_goal /follow_joint_trajectory \
control_msgs/action/FollowJointTrajectory \
"{trajectory: {joint_names: [wheel_left, wheel_right], \
points: [{positions: [0.0, 0.0], time_from_start: {sec: 0}}, \
{positions: [3.14, 3.14], time_from_start: {sec: 2}}]}}"
3.4 ros2_controllers demo¶
上游 ros2_control_demos 仓库用 ros2_control_demo_example_2（差速底盘 DiffBot）与位置控制示例覆盖最常见的配置：
git clone -b humble https://github.com/ros-controls/ros2_control_demos.git \
~/ros2_ws/src/ros2_control_demos
cd ~/ros2_ws && rosdep install --from-paths src --ignore-src -r -y
colcon build --packages-select ros2_control_demo_example_2
source install/setup.bash
ros2 launch ros2_control_demo_example_2 diffbot.launch.py
启动后观察 ros2 control list_controllers 确认状态为 active，ros2 topic hz /odom 确认里程计在发布。
4. 实时性¶
ros2_control 的 controller_manager 在一个独立线程中以固定频率调用 read-update-write 循环。该线程的抖动（实际周期与目标周期的偏差）直接影响控制品质，抖动由三层叠加：内核的调度粒度、控制线程的调度策略、回路内的运行期开销。
内核层由发行版提供选项。Ubuntu 22.04 主仓库提供 linux-image-realtime（PREEMPT_RT 补丁版）与 linux-image-lowlatency；HWE 线只跟到 linux-image-lowlatency-hwe-22.04，没有 realtime 的 HWE 变体。但 PREEMPT_RT 只保证内核态可抢占性，用户态实时行为还取决于应用配置——装上 RT 内核并不等于硬实时：
- 线程使用
SCHED_FIFO调度策略（非默认SCHED_OTHER）； - 绑核（
CPU affinity）避免 IRQ 与用户进程争抢； - 关闭 CPU 频率调节（
cpupower frequency-set -g performance）； - 禁止运行期内存分配。
ros2_control 框架本身不强制实时——默认跑在普通内核、普通线程优先级下，抖动在毫秒级。
4.1 实时线程配置¶
默认情况下 controller_manager 使用普通 SCHED_OTHER 线程。需要更高实时性时，先明确一点：controller_manager（ros2_control 2.x）没有内置的优先级参数，可用的只有 update_rate 与 use_sim_time；SCHED_FIFO 优先级不能在控制器 YAML 里设，属进程级配置，在框架外部完成：
# 1. 给可执行文件授予 Linux 调度权限（一次性配置）
sudo setcap cap_sys_nice+ep /opt/ros/humble/lib/controller_manager/ros2_control_node
# 2. 启动后用 chrt/taskset 设定调度策略与绑核（SCHED_FIFO 优先级范围 1~99，越大越高）
ros2 run controller_manager ros2_control_node &
chrt --pid --fifo 80 $!
taskset --cpu-list 2,3 -p $!
坑：优先级不写在控制器 YAML 里
控制器侧能配的只有 update_rate 与 use_sim_time；SCHED_FIFO 优先级与绑核是进程级配置，只能在框架外用 setcap + chrt + taskset 完成。
较新版本的 ros2_control 在启动器中集成了优先级配置能力，Humble 之后的版本是否提 供对应参数为待核实。
4.2 运行期禁分配¶
控制回路内的动态内存分配（new、malloc、std::vector::push_back 触发扩容）不可预测地耗时，在硬实时回路中是禁忌。realtime_tools 提供无锁缓冲：
##include <realtime_tools/realtime_buffer.h>
##include <realtime_tools/realtime_publisher.h>
// 预分配——在 on_configure 中完成
realtime_tools::RealtimeBuffer<double> goal_buffer;
// update() 中——只读写已分配的 buffer，不触发任何 new/malloc
void MyController::update() {
double* goal = goal_buffer.readFromRT(); // 无锁读取
// ... 计算 ...
}
4.3 周期抖动测量¶
判据级评估——用 cyclictest 测量系统级最坏抖动：
sudo apt install -y rt-tests
cyclictest -t1 -p80 -n -i1000 -l10000 -m
## -t1: 1线程 -p80: SCHED_FIFO 优先级 80 -i1000: 间隔 1ms -l10000: 10000 次 -m: 锁定内存页
输出中 Max 值即为最坏延迟（微秒）；目标 100 Hz 控制回路（周期 10000 μs）的抖动应远小于周期（Max < 1000 μs 为可用）。
ros2_tracing 工具可在 LTTng 层面追踪 rcl 内部事件（DDS receive → callback start → callback end），获取从网络到达到回调完成的完整时间线，精确定位瓶颈段。
5. 要点¶
- ros2_control 四组件（controller_manager、hardware_interface、controller、broadcaster）按 read-update-write 周期执行；分层让控制算法与硬件实现独立演化。
<ros2_control>标签写在 URDF 内声明接口；controller YAML 配置加载类型与参数——接口名必须与 URDF 声明完全匹配，否则 resource conflict。- 自写 hardware_interface 的核心是
read()和write()两个方法——总线协议（串口、SocketCAN、CANopen）封装在这一层，controller 不感知传输细节。 diff_drive_controller与joint_trajectory_controller覆盖最常见的底盘与关节控制需求；spawner 在 launch 中自动完成 lifecycle 转换。- 实时性三要素：线程优先级（SCHED_FIFO）、运行期禁分配（realtime_tools）、CPU 亲和性与频率锁定。Humble 的 controller_manager 不提供优先级参数，进程级调度配置在框架外部完成；PREEMPT_RT 是必要非充分条件。
- 控制周期抖动应量化评估（cyclictest / ros2_tracing），不依赖主观感受判断"够不够实时"。