ROS2_Control

1.ROS2 Control 概述

1.1.ROS2 Control 简述

概念

ros2_control 是 ROS 2 中用于机器人控制的标准框架。它的核心思想是把机器人控制拆成三层：

• 控制器（Controller） ：负责控制算法。例如差速底盘控制器把 cmd_vel 转成左右轮速度，轨迹控制器把关节轨迹转成关节命令。
• 控制器管理器（Controller Manager） ：负责加载、配置、激活、停止控制器，并在固定频率下执行 read -> update -> write 控制循环。
• 硬件接口（Hardware Interface） ：负责和真实硬件、仿真硬件或模拟硬件通信，让控制器不用关心底层串口、CAN、EtherCAT、Gazebo 或自定义驱动。

简单说，ros2_control 是 ROS 2 中“控制器”和“硬件”之间的标准插座。控制器只面对标准接口，硬件只暴露标准接口，中间由 controller_manager 管理。

在 Ubuntu 24.04 + ROS 2 Jazzy 中，ROS 2 的默认安装路径是：

/opt/ros/jazzy

常见 ros2_control 包路径如下：

1.2.ROS2 Control 解决了什么问题

如果不用 ros2_control，机器人控制程序很容易写成这样：

• 节点直接订阅 /cmd_vel；
• 节点自己计算左右轮速度；
• 节点自己读编码器；
• 节点自己写串口或 CAN；
• 换硬件时整套控制节点都要改；
• 从仿真迁移到真机时还要再改一遍。

使用 ros2_control 后，推荐的结构是：

• 控制器负责算法，例如差速运动学、机械臂轨迹插值、PID、夹爪控制。
• 硬件接口负责通信，例如串口协议、CAN 报文、驱动板寄存器、Gazebo 插件。
• URDF 负责声明机器人有哪些 joint、command interface 和 state interface。
• YAML 负责声明加载哪些控制器以及控制器参数。
• controller_manager 负责把它们组合起来。

这样做的好处是：控制器可以复用，硬件可以替换，仿真和真机可以共用大部分上层配置。

2.ROS2 Control 安装

2.1.基础安装

Ubuntu 24.04 使用 ROS 2 Jazzy 时，安装命令如下：

sudo apt update
sudo apt install ros-jazzy-ros2-control ros-jazzy-ros2-controllers

这两个包已经能满足大多数真机控制、控制器开发和基础学习。如果想把本章的调试工具、Gazebo Sim 示例、测试辅助包也装齐，建议继续安装：

sudo apt install \
  ros-jazzy-gz-ros2-control \
  ros-jazzy-gz-ros2-control-demos \
  ros-jazzy-rqt-controller-manager \
  ros-jazzy-rqt-joint-trajectory-controller \
  ros-jazzy-ros2-controllers-test-nodes \
  ros-jazzy-hardware-interface-testing \
  ros-jazzy-joint-state-topic-hardware-interface \
  ros-jazzy-battery-state-broadcaster

其中 ros-jazzy-gz-ros2-control 是 Gazebo Sim 接入 ros2_control 的核心包，ros-jazzy-gz-ros2-control-demos 提供官方可运行示例，两个 rqt 包提供图形化控制器管理和轨迹发送工具。后面几个包不是写普通机器人必需的，但对阅读示例、测试硬件接口、学习 broadcaster 很有帮助。

每次打开新终端后，都需要 source ROS 2 环境：

source /opt/ros/jazzy/setup.bash

2.2.验证安装

可以查看基础包：

ros2 pkg list | grep -E '^(ros2_control|controller_manager|hardware_interface|controller_interface|ros2controlcli)$'

正常情况下可以看到：

controller_interface
controller_manager
hardware_interface
ros2_control
ros2controlcli

可以查看常用控制器：

ros2 pkg list | grep -E '^(joint_state_broadcaster|diff_drive_controller|joint_trajectory_controller|forward_command_controller|position_controllers|velocity_controllers|effort_controllers|pid_controller|mecanum_drive_controller|ackermann_steering_controller|tricycle_controller|omni_wheel_drive_controller)$'

常用输出包括：

ackermann_steering_controller
diff_drive_controller
effort_controllers
forward_command_controller
joint_state_broadcaster
joint_trajectory_controller
mecanum_drive_controller
omni_wheel_drive_controller
pid_controller
position_controllers
tricycle_controller
velocity_controllers

这些包名第一次看会比较抽象，可以先按下面这样理解：

这里有一个很重要的区别：joint_state_broadcaster 是 broadcaster，它只发布状态，不负责让机器人动；diff_drive_controller、joint_trajectory_controller、mecanum_drive_controller 这类才是 controller，会占用 command interface 并向硬件写命令。position_controllers、velocity_controllers、effort_controllers 和 forward_command_controller 更像“直接转发命令”的工具，适合调试硬件接口，但不会替你计算复杂运动学。

如果安装了 Gazebo Sim 和调试辅助包，还可以检查：

ros2 pkg list | grep -E '^(gz_ros2_control|gz_ros2_control_demos|rqt_controller_manager|rqt_joint_trajectory_controller|hardware_interface_testing|joint_state_topic_hardware_interface|battery_state_broadcaster)$'

正常会看到：

battery_state_broadcaster
gz_ros2_control
gz_ros2_control_demos
hardware_interface_testing
joint_state_topic_hardware_interface
rqt_controller_manager
rqt_joint_trajectory_controller

controller_manager 包提供了四个常用可执行文件：

ros2 pkg executables controller_manager

输出应包含：

controller_manager hardware_spawner
controller_manager ros2_control_node
controller_manager spawner
controller_manager unspawner

图形化调试工具可以这样启动：

ros2 run rqt_controller_manager rqt_controller_manager
ros2 run rqt_joint_trajectory_controller rqt_joint_trajectory_controller

rqt_controller_manager 适合查看、加载、配置、激活和停止控制器；rqt_joint_trajectory_controller 适合给 joint_trajectory_controller 手动发送简单关节轨迹。

3.ROS2 Control 核心概念

3.1.command interface 和 state interface

学习 ros2_control 最重要的是理解接口。

• command interface ：控制器写入的目标值。例如电机目标速度、关节目标位置、关节目标力矩。
• state interface ：硬件读回来的状态值。例如编码器位置、当前速度、力矩、电流、温度。

Jazzy 中标准接口名定义在：

/opt/ros/jazzy/include/hardware_interface/hardware_interface/types/hardware_interface_type_values.hpp

常用接口如下：

例如一个轮子关节可以这样声明：

<joint name="left_wheel_joint">
  <command_interface name="velocity"/>
  <state_interface name="position"/>
  <state_interface name="velocity"/>
</joint>

含义是：控制器可以给 left_wheel_joint/velocity 写速度命令；硬件接口要能读回 left_wheel_joint/position 和 left_wheel_joint/velocity。

3.2.控制循环

controller_manager 的核心循环是：

read() -> update() -> write()

具体含义如下：

例如差速底盘：

• read() 读取左右轮编码器，更新左右轮 position/velocity。
• diff_drive_controller.update() 根据 /diff_drive_controller/cmd_vel 和轮子反馈计算左右轮目标速度，并发布里程计。
• write() 把左右轮目标速度写到电机驱动板。

3.3.生命周期状态

ros2_control 的控制器和硬件组件都有生命周期。常见状态如下：

常用流程是：

load -> configure -> activate

spawner 默认会帮控制器完成加载、配置和激活。如果只想加载不激活，可以使用 --load-only 或 --inactive。

4.URDF 中的 ros2_control 标签

4.1.基本结构

ros2_control 的硬件描述写在 URDF 或 xacro 的 <ros2_control> 标签中。

最小结构如下：

<ros2_control name="MyRobotSystem" type="system">
  <hardware>
    <plugin>硬件插件名</plugin>
  </hardware>

  <joint name="关节名">
    <command_interface name="命令接口名"/>
    <state_interface name="状态接口名"/>
  </joint>
</ros2_control>

type 有三种常用取值：

4.2.使用 mock_components 测试

Jazzy 的 hardware_interface 包提供了一个测试用 mock 硬件插件：

/opt/ros/jazzy/share/hardware_interface/mock_components_plugin_description.xml

插件名是：

mock_components/GenericSystem

可以用它先验证控制器配置，而不用立即接真实硬件。

差速底盘示例：

<ros2_control name="DiffBotSystem" type="system">
  <hardware>
    <plugin>mock_components/GenericSystem</plugin>
  </hardware>

  <joint name="left_wheel_joint">
    <command_interface name="velocity"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>

  <joint name="right_wheel_joint">
    <command_interface name="velocity"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>
</ros2_control>

注意三点：

• left_wheel_joint 和 right_wheel_joint 必须是 URDF 中真实存在的关节名。
• 控制器 YAML 里的关节名必须和 URDF 完全一致。
• diff_drive_controller 输出轮子 velocity 命令，所以轮关节必须有 velocity command interface。

4.3.<param> 传硬件参数

真实硬件通常需要串口名、波特率、CAN 设备名、减速比等参数，可以写在 <hardware> 中：

<hardware>
  <plugin>my_robot_hardware/MyRobotSystem</plugin>
  <param name="serial_port">/dev/ttyUSB0</param>
  <param name="baud_rate">115200</param>
  <param name="gear_ratio">30.0</param>
</hardware>

在自定义硬件接口的 on_init(const hardware_interface::HardwareInfo & info) 中，可以通过 info.hardware_parameters 读取这些参数。

5.Controller Manager

5.1.controller_manager 配置

controller_manager 是 ros2_control 的核心节点。它的默认可执行文件是：

ros2 run controller_manager ros2_control_node

在实际工程中通常通过 launch 启动。控制器配置一般写在 YAML 文件中，例如 config/controllers.yaml：

controller_manager:
  ros__parameters:
    update_rate: 100

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    diff_drive_controller:
      type: diff_drive_controller/DiffDriveController

controller_manager 常用参数：

5.2.launch 启动方式

典型 launch 写法如下：

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[
            {"robot_description": "<这里传入完整 URDF 字符串>"},
            "config/controllers.yaml",
        ],
        output="both",
    )

    return LaunchDescription([control_node])

实际项目中，robot_description 通常由 xacro 生成。一般流程是：

xacro 文件 -> robot_description 参数 -> robot_state_publisher
                                  -> ros2_control_node

6.控制器加载与调试命令

6.1.spawner

spawner 用于加载控制器。默认会完成加载、配置、激活三步：

ros2 run controller_manager spawner joint_state_broadcaster
ros2 run controller_manager spawner diff_drive_controller

如果 controller manager 名字不是默认的 /controller_manager，可以用 -c 指定：

ros2 run controller_manager spawner joint_state_broadcaster -c /controller_manager

常用选项如下：

6.2.hardware_spawner

hardware_spawner 用于配置或激活硬件组件：

ros2 run controller_manager hardware_spawner DiffBotSystem --configure
ros2 run controller_manager hardware_spawner DiffBotSystem --activate

常用选项如下：

6.3.ros2 control 命令

ros2controlcli 提供了 ros2 control 命令。

查看控制器：

ros2 control list_controllers

查看控制器详细接口：

ros2 control list_controllers --verbose

查看硬件组件：

ros2 control list_hardware_components

查看硬件接口：

ros2 control list_hardware_interfaces
ros2 control list_hardware_interfaces --verbose

查看可用控制器类型：

ros2 control list_controller_types

切换控制器：

ros2 control switch_controllers --activate diff_drive_controller --deactivate old_controller

卸载控制器：

ros2 control unload_controller diff_drive_controller

生成链式控制器图：

ros2 control view_controller_chains

7.常用控制器

7.1.Joint State Broadcaster

joint_state_broadcaster 用于发布机器人关节状态。插件描述文件是：

/opt/ros/jazzy/share/joint_state_broadcaster/joint_state_plugin.xml

插件名是：

joint_state_broadcaster/JointStateBroadcaster

它会读取 state interfaces，并发布：

常用配置：

joint_state_broadcaster:
  ros__parameters:
    use_local_topics: false
    use_urdf_to_filter: true
    publish_dynamic_joint_states: true

建议几乎所有机器人都启动 joint_state_broadcaster，否则 RViz 中机器人模型通常不会动。

7.2.Diff Drive Controller

diff_drive_controller 用于差速移动机器人。插件描述文件是：

/opt/ros/jazzy/share/diff_drive_controller/diff_drive_plugin.xml

插件名是：

diff_drive_controller/DiffDriveController

它的作用是：

• 订阅速度命令；
• 根据差速运动学计算左右轮目标速度；
• 向左右轮关节的 velocity command interface 写命令；
• 根据左右轮反馈计算里程计；
• 可选发布 odom -> base_link TF。

订阅话题：

如果控制器名是 diff_drive_controller，默认命令话题一般是：

/diff_drive_controller/cmd_vel

发布话题：

最小配置：

diff_drive_controller:
  ros__parameters:
    left_wheel_names: ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]
    wheel_separation: 0.40
    wheel_radius: 0.05
    odom_frame_id: odom
    base_frame_id: base_link
    position_feedback: true
    open_loop: false
    enable_odom_tf: true
    publish_rate: 50.0

如果只是用 mock 硬件学习，可以先用开环：

diff_drive_controller:
  ros__parameters:
    left_wheel_names: ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]
    wheel_separation: 0.40
    wheel_radius: 0.05
    position_feedback: false
    open_loop: true
    enable_odom_tf: true

发送速度命令：

ros2 topic pub /diff_drive_controller/cmd_vel geometry_msgs/msg/TwistStamped "{
  header: {frame_id: base_link},
  twist: {
    linear: {x: 0.2, y: 0.0, z: 0.0},
    angular: {x: 0.0, y: 0.0, z: 0.5}
  }
}"

注意：Jazzy 的 diff_drive_controller 使用 TwistStamped。如果旧教程使用 geometry_msgs/msg/Twist，在 Jazzy 中可能无法直接工作。

7.3.Joint Trajectory Controller

joint_trajectory_controller 常用于机械臂、云台、多关节机器人。插件描述文件是：

/opt/ros/jazzy/share/joint_trajectory_controller/joint_trajectory_plugin.xml

插件名是：

joint_trajectory_controller/JointTrajectoryController

它执行的是关节空间轨迹。常见输入是：

常见配置：

arm_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
      - joint3
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity

URDF 中对应 joint 至少要有：

<joint name="joint1">
  <command_interface name="position"/>
  <state_interface name="position"/>
  <state_interface name="velocity"/>
</joint>

如果机器人底层只接收速度命令，可以把 command_interfaces 改成 velocity。如果底层是力矩控制，可以使用 effort，但这要求硬件接口和控制器参数都匹配。

7.4.Forward Command Controller

forward_command_controller 是一个很适合初学者调试硬件接口的控制器。插件描述文件是：

/opt/ros/jazzy/share/forward_command_controller/forward_command_plugin.xml

插件名包括：

forward_command_controller/ForwardCommandController
forward_command_controller/MultiInterfaceForwardCommandController

它不会做复杂控制，只是把收到的数组命令直接写到指定 joint 的指定 command interface。

示例配置：

forward_velocity_controller:
  ros__parameters:
    joints:
      - left_wheel_joint
      - right_wheel_joint
    interface_name: velocity

对于刚写完硬件接口的人来说，推荐先用 forward controller 测试：

• ros2 control list_hardware_interfaces 确认接口存在；
• 启动 forward_velocity_controller；
• 发送简单速度数组；
• 看硬件接口的 write() 是否收到命令。

7.5.ros2_controllers 中的其他控制器

安装 ros-jazzy-ros2-controllers 后，还会带很多控制器。常见类型如下：

如果只是想确认硬件接口能不能收到命令，优先用 forward_command_controller、position_controllers、velocity_controllers 或 effort_controllers。如果已经有明确运动学模型，再选择 diff_drive_controller、mecanum_drive_controller、ackermann_steering_controller、tricycle_controller 或 omni_wheel_drive_controller。机械臂和多关节机构通常先从 joint_trajectory_controller 学起。

选控制器时先问自己三个问题：

• 我的机器人运动学是不是官方已经支持？
• 我的硬件接收的是 position、velocity 还是 effort？
• 我需要控制器自己算运动学，还是只需要把命令转发给硬件？

如果官方控制器能满足需求，就优先使用官方控制器。只有在运动学特殊、控制律特殊、需要融合特殊传感器时，才建议写自定义控制器。

8.一个差速底盘最小 Bringup

8.1.controllers.yaml

controller_manager:
  ros__parameters:
    update_rate: 100

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    diff_drive_controller:
      type: diff_drive_controller/DiffDriveController

joint_state_broadcaster:
  ros__parameters:
    use_local_topics: false
    publish_dynamic_joint_states: true

diff_drive_controller:
  ros__parameters:
    left_wheel_names: ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]
    wheel_separation: 0.40
    wheel_radius: 0.05
    odom_frame_id: odom
    base_frame_id: base_link
    position_feedback: false
    open_loop: true
    enable_odom_tf: true
    publish_limited_velocity: true

8.2.URDF 关键片段

<ros2_control name="DiffBotSystem" type="system">
  <hardware>
    <plugin>mock_components/GenericSystem</plugin>
  </hardware>

  <joint name="left_wheel_joint">
    <command_interface name="velocity"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>

  <joint name="right_wheel_joint">
    <command_interface name="velocity"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>
</ros2_control>

8.3.启动顺序

推荐顺序如下：

1. 启动 robot_state_publisher。
2. 启动 ros2_control_node。
3. 启动 joint_state_broadcaster。
4. 启动 diff_drive_controller。
5. 发布 /diff_drive_controller/cmd_vel。
6. 查看 /joint_states、/diff_drive_controller/odom 和 /tf。

调试命令如下：

ros2 control list_hardware_components
ros2 control list_hardware_interfaces
ros2 control list_controllers --verbose
ros2 topic echo /joint_states
ros2 topic echo /diff_drive_controller/odom

9.自定义硬件接口

9.1.什么时候需要写硬件接口

如果你的机器人使用真实电机驱动板，通常需要写硬件接口。典型场景包括：

• 串口电机驱动板；
• CAN 总线电机；
• EtherCAT 驱动器；
• 自研 STM32 下位机；
• 自定义传感器数据；
• 非 Gazebo 的自定义仿真器。

如果你只是想验证控制器配置，可以先用 mock_components/GenericSystem。如果你要接真机，就需要继承 hardware_interface::SystemInterface。

9.2.SystemInterface 生命周期

Jazzy 中系统硬件接口的常用函数如下：

在 Jazzy 中，如果接口已经在 URDF 的 <ros2_control> 中声明，SystemInterface 默认会根据 URDF 创建接口。自定义硬件里可以使用：

set_state("left_wheel_joint/position", value);
set_state("left_wheel_joint/velocity", value);
double cmd = get_command("left_wheel_joint/velocity");

这些函数来自 Jazzy 的 hardware_interface::SystemInterface。

9.3.最小硬件接口框架

下面是一个用于差速底盘的硬件接口骨架。它展示结构，不包含具体串口协议：

#include <string>

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

namespace my_robot_hardware
{

class MyDiffBotSystem : public hardware_interface::SystemInterface
{
public:
  hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override
  {
    if (hardware_interface::SystemInterface::on_init(info) !=
      hardware_interface::CallbackReturn::SUCCESS)
    {
      return hardware_interface::CallbackReturn::ERROR;
    }

    serial_port_ = info_.hardware_parameters.at("serial_port");
    return hardware_interface::CallbackReturn::SUCCESS;
  }

  hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override
  {
    // 在这里打开串口、CAN 或网络连接。
    return hardware_interface::CallbackReturn::SUCCESS;
  }

  hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override
  {
    set_command("left_wheel_joint/velocity", 0.0);
    set_command("right_wheel_joint/velocity", 0.0);
    return hardware_interface::CallbackReturn::SUCCESS;
  }

  hardware_interface::return_type read(
    const rclcpp::Time &, const rclcpp::Duration &) override
  {
    // 从编码器读取真实位置和速度。
    set_state("left_wheel_joint/position", left_position_);
    set_state("left_wheel_joint/velocity", left_velocity_);
    set_state("right_wheel_joint/position", right_position_);
    set_state("right_wheel_joint/velocity", right_velocity_);
    return hardware_interface::return_type::OK;
  }

  hardware_interface::return_type write(
    const rclcpp::Time &, const rclcpp::Duration &) override
  {
    const double left_cmd = get_command("left_wheel_joint/velocity");
    const double right_cmd = get_command("right_wheel_joint/velocity");

    // 把 left_cmd 和 right_cmd 写给电机驱动板。
    return hardware_interface::return_type::OK;
  }

private:
  std::string serial_port_;
  double left_position_ = 0.0;
  double left_velocity_ = 0.0;
  double right_position_ = 0.0;
  double right_velocity_ = 0.0;
};

}  // namespace my_robot_hardware

插件导出：

#include "pluginlib/class_list_macros.hpp"

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

对应插件 XML：

<library path="my_robot_hardware">
  <class
    name="my_robot_hardware/MyDiffBotSystem"
    type="my_robot_hardware::MyDiffBotSystem"
    base_class_type="hardware_interface::SystemInterface">
    <description>My differential drive robot hardware interface.</description>
  </class>
</library>

URDF 中使用：

<hardware>
  <plugin>my_robot_hardware/MyDiffBotSystem</plugin>
  <param name="serial_port">/dev/ttyUSB0</param>
</hardware>

9.4.硬件接口调试顺序

写硬件接口时建议按这个顺序调试：

1. 先不用真实电机，确认 pluginlib 能加载硬件插件。
2. 启动 ros2_control_node，运行 ros2 control list_hardware_components。
3. 确认硬件组件能进入 inactive。
4. 确认 list_hardware_interfaces 中有期望的 command/state interfaces。
5. 使用 forward_command_controller 直接给 command interface 发命令。
6. 在 write() 中打印一次命令，确认控制器确实写入。
7. 接真实电机，但先架空轮子或关闭高功率输出。
8. 确认 read() 能正确更新 /joint_states。
9. 最后再接 diff_drive_controller 或 joint_trajectory_controller。

10.自定义运动学控制器

10.1.什么时候需要写控制器

不建议一开始就写自定义控制器。优先判断官方控制器是否够用：

如果你的机器人是四轮差速、麦克纳姆轮、舵轮、特殊并联机构、腿式机器人，官方控制器不一定完全匹配，这时就需要写自定义控制器。

10.2.ControllerInterface 关键函数

Jazzy 中普通控制器继承：

controller_interface::ControllerInterface

必须关注这些函数：

其中 update() 在实时控制循环中执行，不应该做这些事：

• 不要动态分配大量内存；
• 不要访问阻塞 I/O；
• 不要等待锁；
• 不要频繁打印日志；
• 不要在里面创建 publisher/subscriber；
• 不要做可能阻塞的参数读取。

订阅 ROS 话题时，常用做法是普通回调写入 realtime_tools::RealtimeBuffer，update() 中只读这个 buffer。

10.3.一个自定义底盘控制器的思路

假设要写一个自定义底盘控制器，输入是 cmd_vel，输出是四个轮子的速度命令。核心步骤是：

1. 参数中声明四个轮子的 joint 名。
2. command_interface_configuration() 返回四个 <joint>/velocity。
3. state_interface_configuration() 返回需要读取的 <joint>/position 或 <joint>/velocity。
4. on_configure() 创建 cmd_vel 订阅者。
5. 订阅者把最新速度命令写入实时 buffer。
6. update() 读取最新速度命令。
7. 根据运动学模型计算各轮速度。
8. 把结果写入 command_interfaces_。

伪代码如下：

controller_interface::InterfaceConfiguration
MyKinematicsController::command_interface_configuration() const
{
  controller_interface::InterfaceConfiguration config;
  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
  config.names = {
    "front_left_wheel_joint/velocity",
    "front_right_wheel_joint/velocity",
    "rear_left_wheel_joint/velocity",
    "rear_right_wheel_joint/velocity"
  };
  return config;
}

controller_interface::return_type MyKinematicsController::update(
  const rclcpp::Time &, const rclcpp::Duration &)
{
  const auto cmd = *command_buffer_.readFromRT();

  const double vx = cmd.linear.x;
  const double wz = cmd.angular.z;

  const double left = vx - wz * wheel_separation_ * 0.5;
  const double right = vx + wz * wheel_separation_ * 0.5;

  command_interfaces_[0].set_value(left / wheel_radius_);
  command_interfaces_[1].set_value(right / wheel_radius_);
  command_interfaces_[2].set_value(left / wheel_radius_);
  command_interfaces_[3].set_value(right / wheel_radius_);

  return controller_interface::return_type::OK;
}

这段代码只是运动学控制器核心逻辑。完整控制器还需要参数声明、订阅者、接口排序检查、超时保护、插件导出和 CMake 配置。

10.4.自定义控制器 package.xml

常见依赖如下：

<depend>controller_interface</depend>
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>realtime_tools</depend>
<depend>geometry_msgs</depend>

如果要发布里程计，还需要：

<depend>nav_msgs</depend>
<depend>tf2</depend>
<depend>tf2_msgs</depend>
<depend>tf2_ros</depend>

10.5.自定义控制器 CMakeLists

核心写法如下：

add_library(my_kinematics_controller SHARED
  src/my_kinematics_controller.cpp
)

ament_target_dependencies(my_kinematics_controller
  controller_interface
  hardware_interface
  pluginlib
  rclcpp
  rclcpp_lifecycle
  realtime_tools
  geometry_msgs
)

pluginlib_export_plugin_description_file(
  controller_interface
  my_kinematics_controller.xml
)

install(
  TARGETS my_kinematics_controller
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
)

install(
  FILES my_kinematics_controller.xml
  DESTINATION share/${PROJECT_NAME}
)

插件 XML：

<library path="my_kinematics_controller">
  <class
    name="my_kinematics_controller/MyKinematicsController"
    type="my_kinematics_controller::MyKinematicsController"
    base_class_type="controller_interface::ControllerInterface">
    <description>My custom mobile robot kinematics controller.</description>
  </class>
</library>

YAML 中加载：

controller_manager:
  ros__parameters:
    my_kinematics_controller:
      type: my_kinematics_controller/MyKinematicsController

my_kinematics_controller:
  ros__parameters:
    wheel_radius: 0.05
    wheel_separation: 0.40

10.6.普通控制器还是链式控制器

Jazzy 中控制器分两类：

初学者建议先写普通控制器。只有当你需要多个控制器串联时，再研究链式控制器。例如：

导航速度命令 -> 运动学控制器 -> PID 控制器 -> 硬件接口

链式控制器可以用：

ros2 control view_controller_chains

查看控制器链路。

11.Gazebo 与 ROS2 Control

ros2_control 本身不是仿真器。它只是控制框架。要在 Gazebo Sim 中使用，需要 gz_ros2_control 插件。

安装：

sudo apt install ros-jazzy-gz-ros2-control ros-jazzy-gz-ros2-control-demos

Gazebo 集成时仍然需要：

• URDF 中的 <ros2_control>；
• 控制器 YAML；
• controller_manager；
• joint_state_broadcaster 和具体控制器。

区别在于 <hardware><plugin>...</plugin></hardware> 通常换成 Gazebo 对应硬件插件，而不是 mock_components/GenericSystem 或你的真实硬件插件。

Jazzy 中 gz_ros2_control 的硬件插件描述文件是：

/opt/ros/jazzy/share/gz_ros2_control/gz_hardware_plugins.xml

Gazebo Sim 系统硬件插件名是：

gz_ros2_control/GazeboSimSystem

URDF 或 xacro 中的 <ros2_control> 关键片段通常写成：

<ros2_control name="GazeboSimSystem" type="system">
  <hardware>
    <plugin>gz_ros2_control/GazeboSimSystem</plugin>
  </hardware>

  <joint name="left_wheel_joint">
    <command_interface name="velocity"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>

  <joint name="right_wheel_joint">
    <command_interface name="velocity"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>
</ros2_control>

同时还要在机器人描述中加入 Gazebo 系统插件，让 Gazebo Sim 创建 ros2_control 资源管理器并加载控制器参数：

<gazebo>
  <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
    <parameters>$(find my_robot_bringup)/config/controllers.yaml</parameters>
  </plugin>
</gazebo>

安装 gz_ros2_control_demos 后，可以直接参考官方示例：

ros2 launch gz_ros2_control_demos diff_drive_example.launch.py
ros2 launch gz_ros2_control_demos cart_example_position.launch.py
ros2 launch gz_ros2_control_demos cart_example_velocity.launch.py
ros2 launch gz_ros2_control_demos cart_example_effort.launch.py
ros2 launch gz_ros2_control_demos mecanum_drive_example.launch.py
ros2 launch gz_ros2_control_demos ackermann_drive_example.launch.py
ros2 launch gz_ros2_control_demos tricycle_drive_example.launch.py

这些示例文件安装在：

/opt/ros/jazzy/share/gz_ros2_control_demos

比较重要的文件包括：

学习 Gazebo 集成时，不要只看 launch 文件。更推荐按这个顺序读：

1. 先看 urdf/test_diff_drive.xacro.urdf 中 <ros2_control> 和 <gazebo> 插件。
2. 再看 config/diff_drive_controller.yaml 中控制器参数。
3. 最后看 launch/diff_drive_example.launch.py 如何把机器人描述、Gazebo 和 spawner 连起来。

建议学习顺序是：

1. 先用 mock_components/GenericSystem 跑通控制器。
2. 再接 Gazebo。
3. 最后接真实硬件。

这样出错时更容易定位问题。

12.常见问题

12.1.控制器加载失败

优先检查：

ros2 control list_controller_types

如果你的控制器类型不在列表里，通常是：

• 插件 XML 没有安装；
• pluginlib_export_plugin_description_file() 写错；
• YAML 中 type 字段和插件 XML 中的 name 不一致；
• 没有 source 工作空间：

source install/setup.bash

12.2.控制器激活失败

查看接口：

ros2 control list_hardware_interfaces --verbose
ros2 control list_controllers --verbose

常见原因：

• 控制器要的 command interface 不存在；
• 控制器要的 state interface 不存在；
• 另一个控制器已经占用了同一个 command interface；
• URDF 里的 joint 名和 YAML 里的 joint 名不一致；
• 硬件组件还没有激活。

12.3./joint_states 没有数据

检查：

ros2 control list_controllers
ros2 topic echo /dynamic_joint_states
ros2 topic echo /joint_states

常见原因：

• 没有启动 joint_state_broadcaster；
• 硬件接口没有 state interfaces；
• use_urdf_to_filter=true 时，URDF 中没有对应 joint；
• 硬件 read() 没有更新 state。

12.4.cmd_vel 发了但底盘不动

Jazzy 的 diff_drive_controller 默认订阅 TwistStamped：

ros2 topic info /diff_drive_controller/cmd_vel

确认消息类型是：

geometry_msgs/msg/TwistStamped

还要检查：

• 话题名是不是 /diff_drive_controller/cmd_vel；
• 控制器是否 active；
• 轮关节 velocity command interface 是否存在；
• left_wheel_names、right_wheel_names 是否写对；
• cmd_vel_timeout 是否太短；
• 真机硬件接口 write() 是否真的把命令发给电机。

12.5.里程计方向不对

优先检查：

• 左右轮 joint 是否写反；
• 轮子正方向是否与 URDF 轴方向一致；
• wheel_radius 是否正确；
• wheel_separation 是否正确；
• 编码器位置单位是否是弧度；
• 硬件接口中的速度单位是否是 rad/s。

12.6.TF 冲突

如果系统中已有其他节点发布 odom -> base_link，不要再让 diff_drive_controller 发布同一段 TF：

diff_drive_controller:
  ros__parameters:
    enable_odom_tf: false

13.学习路线建议

建议按下面顺序学习：

1. 看懂 command interface 和 state interface。
2. 用 mock_components/GenericSystem 跑通 joint_state_broadcaster。
3. 用 forward_command_controller 直接写关节命令。
4. 用 diff_drive_controller 跑通差速底盘。
5. 用 joint_trajectory_controller 跑通机械臂或多关节模型。
6. 写一个最小 SystemInterface，先只打印命令。
7. 接真实串口/CAN，让 read() 更新 /joint_states。
8. 再接官方控制器。
9. 最后再写自定义运动学控制器。

不要一开始就同时写硬件接口、运动学控制器、仿真插件和导航。这样一旦出错，很难判断是 URDF、YAML、控制器、硬件接口还是底层通信的问题。

14.参考资料

• ROS2 Control Jazzy 官方文档：https://control.ros.org/jazzy/index.html
• Getting Started：https://control.ros.org/jazzy/doc/getting_started/getting_started.html
• Controller Manager：https://control.ros.org/jazzy/doc/ros2_control/controller_manager/doc/userdoc.html
• Hardware Interface：https://control.ros.org/jazzy/doc/ros2_control/hardware_interface/doc/hardware_interface_types_userdoc.html
• Joint State Broadcaster：https://control.ros.org/jazzy/doc/ros2_controllers/joint_state_broadcaster/doc/userdoc.html
• Diff Drive Controller：https://control.ros.org/jazzy/doc/ros2_controllers/diff_drive_controller/doc/userdoc.html
• Joint Trajectory Controller：https://control.ros.org/jazzy/doc/ros2_controllers/joint_trajectory_controller/doc/userdoc.html
• ros2_control GitHub：https://github.com/ros-controls/ros2_control
• ros2_controllers GitHub：https://github.com/ros-controls/ros2_controllers