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