ROS2_Control

ROS2 Control 概述

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 包路徑如下：

ROS2 Control 解決了什麼問題

如果不用 ros2_control，機器人控制程序很容易寫成這樣：

• 節點直接訂閱 /cmd_vel；
• 節點自己計算左右輪速度；
• 節點自己讀編碼器；
• 節點自己寫串口或 CAN；
• 換硬件時整套控制節點都要改；
• 從仿真遷移到真機時還要再改一遍。

使用 ros2_control 後，推薦的結構是：

• 控制器負責算法，例如差速運動學、機械臂軌跡插值、PID、夾爪控制。
• 硬件接口負責通信，例如串口協議、CAN 報文、驅動板寄存器、Gazebo 插件。
• URDF 負責聲明機器人有哪些 joint、command interface 和 state interface。
• YAML 負責聲明加載哪些控制器以及控制器參數。
• controller_manager 負責把它們組合起來。

這樣做的好處是：控制器可以複用，硬件可以替換，仿真和真機可以共用大部分上層配置。

ROS2 Control 安裝

基礎安裝

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

驗證安裝

可以查看基礎包：

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 手動發送簡單關節軌跡。

ROS2 Control 核心概念

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。

控制循環

controller_manager 的核心循環是：

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

具體含義如下：

例如差速底盤：

• read() 讀取左右輪編碼器，更新左右輪 position/velocity。
• diff_drive_controller.update() 根據 /diff_drive_controller/cmd_vel 和輪子反饋計算左右輪目標速度，併發布里程計。
• write() 把左右輪目標速度寫到電機驅動板。

生命週期狀態

ros2_control 的控制器和硬件組件都有生命週期。常見狀態如下：

常用流程是：

load -> configure -> activate

spawner 默認會幫控制器完成加載、配置和激活。如果只想加載不激活，可以使用 --load-only 或 --inactive。

URDF 中的 ros2_control 標籤

基本結構

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 有三種常用取值：

使用 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。

<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 讀取這些參數。

Controller Manager

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 常用參數：

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

控制器加載與調試命令

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

常用選項如下：

hardware_spawner

hardware_spawner 用於配置或激活硬件組件：

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

常用選項如下：

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

常用控制器

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 中機器人模型通常不會動。

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 中可能無法直接工作。

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，但這要求硬件接口和控制器參數都匹配。

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() 是否收到命令。

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？
• 我需要控制器自己算運動學，還是隻需要把命令轉發給硬件？

如果官方控制器能滿足需求，就優先使用官方控制器。只有在運動學特殊、控制律特殊、需要融合特殊傳感器時，才建議寫自定義控制器。

一個差速底盤最小 Bringup

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

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>

啟動順序

推薦順序如下：

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

自定義硬件接口

什麼時候需要寫硬件接口

如果你的機器人使用真實電機驅動板，通常需要寫硬件接口。典型場景包括：

• 串口電機驅動板；
• CAN 總線電機；
• EtherCAT 驅動器；
• 自研 STM32 下位機；
• 自定義傳感器數據；
• 非 Gazebo 的自定義仿真器。

如果你只是想驗證控制器配置，可以先用 mock_components/GenericSystem。如果你要接真機，就需要繼承 hardware_interface::SystemInterface。

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。

最小硬件接口框架

下面是一個用於差速底盤的硬件接口骨架。它展示結構，不包含具體串口協議：

#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>

硬件接口調試順序

寫硬件接口時建議按這個順序調試：

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。

自定義運動學控制器

什麼時候需要寫控制器

不建議一開始就寫自定義控制器。優先判斷官方控制器是否夠用：

如果你的機器人是四輪差速、麥克納姆輪、舵輪、特殊並聯機構、腿式機器人，官方控制器不一定完全匹配，這時就需要寫自定義控制器。

ControllerInterface 關鍵函數

Jazzy 中普通控制器繼承：

controller_interface::ControllerInterface

必須關注這些函數：

其中 update() 在實時控制循環中執行，不應該做這些事：

• 不要動態分配大量內存；
• 不要訪問阻塞 I/O；
• 不要等待鎖；
• 不要頻繁打印日誌；
• 不要在裡面創建 publisher/subscriber；
• 不要做可能阻塞的參數讀取。

訂閱 ROS 話題時，常用做法是普通回調寫入 realtime_tools::RealtimeBuffer，update() 中只讀這個 buffer。

一個自定義底盤控制器的思路

假設要寫一個自定義底盤控制器，輸入是 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 配置。

自定義控制器 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>

自定義控制器 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

普通控制器還是鏈式控制器

Jazzy 中控制器分兩類：

初學者建議先寫普通控制器。只有當你需要多個控制器串聯時，再研究鏈式控制器。例如：

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

鏈式控制器可以用：

ros2 control view_controller_chains

查看控制器鏈路。

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. 最後接真實硬件。

這樣出錯時更容易定位問題。

常見問題

控制器加載失敗

優先檢查：

ros2 control list_controller_types

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

• 插件 XML 沒有安裝；
• pluginlib_export_plugin_description_file() 寫錯；
• YAML 中 type 字段和插件 XML 中的 name 不一致；
• 沒有 source 工作空間：

source install/setup.bash

控制器激活失敗

查看接口：

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

常見原因：

• 控制器要的 command interface 不存在；
• 控制器要的 state interface 不存在；
• 另一個控制器已經佔用了同一個 command interface；
• URDF 裡的 joint 名和 YAML 裡的 joint 名不一致；
• 硬件組件還沒有激活。

/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。

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() 是否真的把命令發給電機。

里程計方向不對

優先檢查：

• 左右輪 joint 是否寫反；
• 輪子正方向是否與 URDF 軸方向一致；
• wheel_radius 是否正確；
• wheel_separation 是否正確；
• 編碼器位置單位是否是弧度；
• 硬件接口中的速度單位是否是 rad/s。

TF 衝突

如果系統中已有其他節點發布 odom -> base_link，不要再讓 diff_drive_controller 發佈同一段 TF：

diff_drive_controller:
  ros__parameters:
    enable_odom_tf: false

學習路線建議

建議按下面順序學習：

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、控制器、硬件接口還是底層通信的問題。

參考資料

• 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