ROS 2 từ A đến Z (Phần 4): ros2_control và Hardware

ros2_control — Cầu nối giữa ROS 2 và phần cứng thực

Khi bạn đã chạy được robot trong simulation với Nav2, câu hỏi tiếp theo luôn là: làm sao để điều khiển motor thực? Đó chính là lúc ros2_control phát huy tác dụng. Framework này cung cấp một lớp trừu tượng chuẩn hoá giữa controller (thuật toán điều khiển) và hardware interface (giao tiếp phần cứng), giúp bạn tách biệt logic điều khiển khỏi driver cụ thể.

Bài này tiếp nối chuỗi series ROS 2 — nếu bạn chưa đọc, hãy bắt đầu từ Phần 1: Cài đặt và Node đầu tiên, Phần 2: Topics, Services và Actions, và Phần 3: Nav2.

Kiến trúc ros2_control

ros2_control gồm 3 thành phần chính hoạt động cùng nhau:

┌─────────────────────────────────────────────────┐
│                Controller Manager                │
│  (Load, configure, activate controllers)         │
├──────────────┬──────────────┬───────────────────┤
│ joint_state  │ forward_cmd  │ diff_drive        │
│ _broadcaster │ _controller  │ _controller       │
│              │              │                   │
│ (đọc state)  │ (gửi command)│ (diff drive)      │
└──────┬───────┴──────┬───────┴──────┬────────────┘
       │              │              │
       │    Command & State interfaces
       │              │              │
┌──────▼──────────────▼──────────────▼────────────┐
│              Hardware Interface                   │
│  (SystemInterface / ActuatorInterface /           │
│   SensorInterface)                               │
├─────────────────────────────────────────────────┤
│  read() → đọc encoder, IMU                       │
│  write() → gửi PWM, velocity command             │
└──────────────────┬──────────────────────────────┘
                   │
          ┌────────▼────────┐
          │   Phần cứng     │
          │  (Motor, Encoder │
          │   IMU, Gripper)  │
          └─────────────────┘

Các khái niệm cốt lõi:

Khái niệm	Vai trò
Controller Manager	Node trung tâm, quản lý lifecycle của tất cả controllers
Hardware Interface	Plugin giao tiếp trực tiếp với phần cứng qua serial/CAN/SPI
State Interface	Đọc trạng thái (position, velocity, effort) từ encoder/sensor
Command Interface	Gửi lệnh (position, velocity, effort) xuống motor driver
Controller	Thuật toán điều khiển — nhận state, tính toán, gửi command

Cài đặt ros2_control

# Cài đặt ros2_control packages
sudo apt install -y \
  ros-humble-ros2-control \
  ros-humble-ros2-controllers \
  ros-humble-controller-manager \
  ros-humble-joint-state-broadcaster \
  ros-humble-forward-command-controller \
  ros-humble-diff-drive-controller

# Nếu cần mock hardware để test
sudo apt install -y ros-humble-ros2-control-test-assets

Bước 1: Mô tả phần cứng trong URDF

ros2_control sử dụng tag <ros2_control> trong URDF để khai báo hardware interface. Đây là ví dụ cho robot 2 bánh differential drive với DC motor và encoder:

<!-- ~/ros2_ws/src/my_robot_hardware/urdf/robot.urdf.xacro -->
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="my_diff_robot">

  <!-- Base link -->
  <link name="base_link">
    <visual>
      <geometry><box size="0.3 0.2 0.1"/></geometry>
    </visual>
  </link>

  <!-- Left wheel -->
  <link name="left_wheel">
    <visual>
      <geometry><cylinder radius="0.05" length="0.04"/></geometry>
    </visual>
  </link>
  <joint name="left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel"/>
    <origin xyz="0.0 0.12 0.0" rpy="${-pi/2} 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

  <!-- Right wheel (tương tự) -->
  <link name="right_wheel">
    <visual>
      <geometry><cylinder radius="0.05" length="0.04"/></geometry>
    </visual>
  </link>
  <joint name="right_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel"/>
    <origin xyz="0.0 -0.12 0.0" rpy="${-pi/2} 0 0"/>
    <axis xyz="0 0 1"/>
  </joint>

  <!-- ros2_control hardware interface -->
  <ros2_control name="DiffDriveSystem" type="system">

    <!-- Plugin: dùng custom hoặc mock -->
    <hardware>
      <plugin>my_robot_hardware/DiffDriveHardware</plugin>
      <param name="serial_port">/dev/ttyUSB0</param>
      <param name="baud_rate">115200</param>
      <param name="encoder_ticks_per_rev">1440</param>
      <param name="wheel_radius">0.05</param>
    </hardware>

    <!-- Left wheel joint -->
    <joint name="left_wheel_joint">
      <command_interface name="velocity">
        <param name="min">-10.0</param>
        <param name="max">10.0</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>

    <!-- Right wheel joint -->
    <joint name="right_wheel_joint">
      <command_interface name="velocity">
        <param name="min">-10.0</param>
        <param name="max">10.0</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>

  </ros2_control>

</robot>

Giải thích:

type="system" — SystemInterface quản lý nhiều joint cùng lúc (phù hợp cho diff drive, robot arm)
command_interface name="velocity" — controller sẽ gửi velocity command
state_interface name="position" và "velocity" — hardware trả về position và velocity từ encoder
Các param trong <hardware> sẽ được đọc trong code C++ của plugin

Bước 2: Viết Custom Hardware Interface (C++)

Đây là phần quan trọng nhất — viết plugin giao tiếp với phần cứng thật qua serial:

// ~/ros2_ws/src/my_robot_hardware/src/diff_drive_hardware.cpp
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/rclcpp.hpp"
#include <serial/serial.h>  // libserial

namespace my_robot_hardware
{

class DiffDriveHardware : public hardware_interface::SystemInterface
{
public:
  // Khởi tạo — đọc params từ URDF
  hardware_interface::CallbackReturn on_init(
    const hardware_interface::HardwareInfo & info) override
  {
    if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
      return CallbackReturn::ERROR;
    }

    // Đọc params
    serial_port_ = info_.hardware_parameters["serial_port"];
    baud_rate_ = std::stoi(info_.hardware_parameters["baud_rate"]);
    ticks_per_rev_ = std::stoi(info_.hardware_parameters["encoder_ticks_per_rev"]);
    wheel_radius_ = std::stod(info_.hardware_parameters["wheel_radius"]);

    // Khởi tạo biến
    hw_positions_.resize(2, 0.0);
    hw_velocities_.resize(2, 0.0);
    hw_commands_.resize(2, 0.0);

    return CallbackReturn::SUCCESS;
  }

  // Kết nối serial khi activate
  hardware_interface::CallbackReturn on_activate(
    const rclcpp_lifecycle::State &) override
  {
    try {
      serial_.setPort(serial_port_);
      serial_.setBaudrate(baud_rate_);
      serial::Timeout timeout = serial::Timeout::simpleTimeout(1000);
      serial_.setTimeout(timeout);
      serial_.open();
      RCLCPP_INFO(rclcpp::get_logger("DiffDriveHardware"),
                  "Connected to %s at %d baud", serial_port_.c_str(), baud_rate_);
    } catch (const serial::IOException & e) {
      RCLCPP_ERROR(rclcpp::get_logger("DiffDriveHardware"),
                   "Cannot open serial port: %s", e.what());
      return CallbackReturn::ERROR;
    }
    return CallbackReturn::SUCCESS;
  }

  // Ngắt serial khi deactivate
  hardware_interface::CallbackReturn on_deactivate(
    const rclcpp_lifecycle::State &) override
  {
    if (serial_.isOpen()) {
      serial_.close();
    }
    return CallbackReturn::SUCCESS;
  }

  // Export state interfaces cho controller đọc
  std::vector<hardware_interface::StateInterface> export_state_interfaces() override
  {
    std::vector<hardware_interface::StateInterface> interfaces;
    for (size_t i = 0; i < 2; ++i) {
      interfaces.emplace_back(
        info_.joints[i].name, "position", &hw_positions_[i]);
      interfaces.emplace_back(
        info_.joints[i].name, "velocity", &hw_velocities_[i]);
    }
    return interfaces;
  }

  // Export command interfaces cho controller ghi
  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override
  {
    std::vector<hardware_interface::CommandInterface> interfaces;
    for (size_t i = 0; i < 2; ++i) {
      interfaces.emplace_back(
        info_.joints[i].name, "velocity", &hw_commands_[i]);
    }
    return interfaces;
  }

  // ĐỌC encoder — gọi mỗi control loop cycle
  hardware_interface::return_type read(
    const rclcpp::Time &, const rclcpp::Duration & period) override
  {
    if (!serial_.isOpen()) return hardware_interface::return_type::ERROR;

    // Gửi lệnh đọc encoder (protocol tuỳ firmware)
    serial_.write("GET_ENCODERS\n");
    std::string response = serial_.readline();

    // Parse response: "left_ticks right_ticks"
    int left_ticks, right_ticks;
    if (sscanf(response.c_str(), "%d %d", &left_ticks, &right_ticks) == 2) {
      double dt = period.seconds();

      // Tính position (radians) từ encoder ticks
      double left_rad = (2.0 * M_PI * left_ticks) / ticks_per_rev_;
      double right_rad = (2.0 * M_PI * right_ticks) / ticks_per_rev_;

      // Tính velocity (rad/s)
      hw_velocities_[0] = (left_rad - hw_positions_[0]) / dt;
      hw_velocities_[1] = (right_rad - hw_positions_[1]) / dt;

      hw_positions_[0] = left_rad;
      hw_positions_[1] = right_rad;
    }

    return hardware_interface::return_type::OK;
  }

  // GHI velocity command xuống motor — gọi mỗi control loop cycle
  hardware_interface::return_type write(
    const rclcpp::Time &, const rclcpp::Duration &) override
  {
    if (!serial_.isOpen()) return hardware_interface::return_type::ERROR;

    // Gửi velocity command qua serial (rad/s → RPM hoặc PWM tuỳ firmware)
    double left_rpm = hw_commands_[0] * 60.0 / (2.0 * M_PI);
    double right_rpm = hw_commands_[1] * 60.0 / (2.0 * M_PI);

    char cmd[64];
    snprintf(cmd, sizeof(cmd), "SET_SPEED %.2f %.2f\n", left_rpm, right_rpm);
    serial_.write(cmd);

    return hardware_interface::return_type::OK;
  }

private:
  serial::Serial serial_;
  std::string serial_port_;
  int baud_rate_;
  int ticks_per_rev_;
  double wheel_radius_;

  std::vector<double> hw_positions_;
  std::vector<double> hw_velocities_;
  std::vector<double> hw_commands_;
};

}  // namespace my_robot_hardware

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
  my_robot_hardware::DiffDriveHardware,
  hardware_interface::SystemInterface)

Đăng ký plugin

<!-- ~/ros2_ws/src/my_robot_hardware/my_robot_hardware.xml -->
<library path="my_robot_hardware">
  <class name="my_robot_hardware/DiffDriveHardware"
         type="my_robot_hardware::DiffDriveHardware"
         base_class_type="hardware_interface::SystemInterface">
    <description>Hardware interface for differential drive robot via serial</description>
  </class>
</library>

Bước 3: Cấu hình Controllers

File YAML cấu hình controller_manager và các controller cần dùng:

# ~/ros2_ws/src/my_robot_hardware/config/controllers.yaml
controller_manager:
  ros__parameters:
    update_rate: 50  # Hz — tần số control loop

    # Khai báo controllers
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    diff_drive_controller:
      type: diff_drive_controller/DiffDriveController

# Cấu hình JointStateBroadcaster — publish joint states lên /joint_states
joint_state_broadcaster:
  ros__parameters:
    joints:
      - left_wheel_joint
      - right_wheel_joint
    interfaces:
      - position
      - velocity

# Cấu hình DiffDriveController — nhận /cmd_vel, gửi velocity cho 2 bánh
diff_drive_controller:
  ros__parameters:
    left_wheel_names: ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]

    # Thông số cơ học robot
    wheel_separation: 0.24     # Khoảng cách 2 bánh (m)
    wheel_radius: 0.05         # Bán kính bánh (m)

    # Giới hạn tốc độ
    linear.x.max_velocity: 0.5        # m/s
    linear.x.min_velocity: -0.3
    angular.z.max_velocity: 2.0       # rad/s
    angular.z.min_velocity: -2.0

    # Giới hạn gia tốc
    linear.x.max_acceleration: 1.0
    angular.z.max_acceleration: 3.0

    # Publish odometry
    publish_rate: 50.0
    odom_frame_id: odom
    base_frame_id: base_footprint
    enable_odom_tf: true

    # Timeout — dừng motor nếu không nhận cmd_vel
    cmd_vel_timeout: 0.5

Tham số quan trọng cần đo chính xác:

wheel_separation — đo khoảng cách tâm 2 bánh
wheel_radius — đo bán kính bánh xe
Sai lệch nhỏ sẽ gây robot đi cong hoặc odometry drift

Bước 4: Launch File

# ~/ros2_ws/src/my_robot_hardware/launch/robot_bringup.launch.py
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import RegisterEventHandler, TimerAction
from launch.event_handlers import OnProcessStart
from ament_index_python.packages import get_package_share_directory
import xacro


def generate_launch_description():
    pkg_path = get_package_share_directory('my_robot_hardware')

    # Xử lý URDF từ xacro
    xacro_file = os.path.join(pkg_path, 'urdf', 'robot.urdf.xacro')
    robot_description = xacro.process_file(xacro_file).toxml()

    # Robot State Publisher — publish TF từ URDF
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_description}],
    )

    # Controller Manager — load hardware interface + controllers
    controller_manager = Node(
        package='controller_manager',
        executable='ros2_control_node',
        parameters=[
            {'robot_description': robot_description},
            os.path.join(pkg_path, 'config', 'controllers.yaml'),
        ],
        output='screen',
    )

    # Spawn controllers sau khi controller_manager đã chạy
    spawn_jsb = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
    )

    spawn_ddc = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['diff_drive_controller', '--controller-manager', '/controller_manager'],
    )

    # Đợi controller_manager sẵn sàng rồi mới spawn
    delayed_spawners = RegisterEventHandler(
        event_handler=OnProcessStart(
            target_action=controller_manager,
            on_start=[
                TimerAction(
                    period=3.0,
                    actions=[spawn_jsb, spawn_ddc],
                ),
            ],
        )
    )

    return LaunchDescription([
        robot_state_publisher,
        controller_manager,
        delayed_spawners,
    ])

Bước 5: Test với Mock Hardware

Trước khi chạy trên phần cứng thật, luôn test với mock hardware. Trong URDF, thay plugin:

<hardware>
  <!-- Thay custom plugin bằng mock -->
  <plugin>mock_components/GenericSystem</plugin>
  <param name="mock_sensor_commands">false</param>
  <param name="state_following_offset">0.0</param>
</hardware>

# Launch với mock
ros2 launch my_robot_hardware robot_bringup.launch.py

# Kiểm tra controllers
ros2 control list_controllers
# joint_state_broadcaster  [active]
# diff_drive_controller    [active]

# Kiểm tra hardware interfaces
ros2 control list_hardware_interfaces
# command interfaces:
#   left_wheel_joint/velocity [available] [claimed]
#   right_wheel_joint/velocity [available] [claimed]
# state interfaces:
#   left_wheel_joint/position
#   left_wheel_joint/velocity
#   right_wheel_joint/position
#   right_wheel_joint/velocity

# Gửi cmd_vel test
ros2 topic pub /diff_drive_controller/cmd_vel_unstamped \
  geometry_msgs/msg/Twist \
  "{linear: {x: 0.2}, angular: {z: 0.0}}"

# Xem joint states
ros2 topic echo /joint_states

Bước 6: Chạy trên phần cứng thật

Khi mock test OK, chuyển sang hardware thật:

Kiểm tra serial: ls /dev/ttyUSB* hoặc /dev/ttyACM*
Cấp quyền: sudo chmod 666 /dev/ttyUSB0 hoặc thêm user vào group dialout
Đổi plugin trong URDF về my_robot_hardware/DiffDriveHardware
Launch và test từ từ — bắt đầu với velocity thấp

# Cấp quyền serial
sudo usermod -a -G dialout $USER
# Logout/login lại

# Launch
ros2 launch my_robot_hardware robot_bringup.launch.py

# Test với tốc độ thấp trước
ros2 topic pub --once /diff_drive_controller/cmd_vel_unstamped \
  geometry_msgs/msg/Twist "{linear: {x: 0.05}}"

Firmware phía Arduino (tham khảo)

Phía microcontroller (Arduino/ESP32), firmware cần implement protocol tương ứng:

// Arduino firmware (sketch tham khảo)
volatile long left_ticks = 0;
volatile long right_ticks = 0;

void setup() {
  Serial.begin(115200);
  // Setup encoder interrupts
  attachInterrupt(digitalPinToInterrupt(2), leftEncoderISR, CHANGE);
  attachInterrupt(digitalPinToInterrupt(3), rightEncoderISR, CHANGE);
  // Setup motor pins
  pinMode(5, OUTPUT); // Left PWM
  pinMode(6, OUTPUT); // Right PWM
}

void loop() {
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');

    if (cmd == "GET_ENCODERS") {
      Serial.print(left_ticks);
      Serial.print(" ");
      Serial.println(right_ticks);
    }
    else if (cmd.startsWith("SET_SPEED")) {
      float left_rpm, right_rpm;
      sscanf(cmd.c_str(), "SET_SPEED %f %f", &left_rpm, &right_rpm);
      setMotorSpeed(left_rpm, right_rpm);
    }
  }
}

Troubleshooting thường gặp

Controller không activate được

# Kiểm tra hardware interface đã load chưa
ros2 control list_hardware_components
# Nếu state = unconfigured → URDF sai hoặc plugin không tìm thấy

# Kiểm tra log
ros2 topic echo /diagnostics

Encoder đọc sai giá trị

Kiểm tra encoder_ticks_per_rev — tính cả gear ratio
Kiểm tra hướng quay (sign) — đổi dấu nếu robot đi lùi khi command forward
Tần số read() phải > tần số encoder thay đổi

Odometry drift

Đo lại wheel_separation và wheel_radius chính xác đến mm
Calibrate bằng cách cho robot đi thẳng 1m, đo sai lệch
Cho robot quay 360 độ, đo sai lệch góc

Tổng kết

Trong bài này, bạn đã nắm được:

Kiến trúc ros2_control: Controller Manager, Hardware Interface, Controllers
Viết custom SystemInterface cho DC motor + encoder qua serial
Khai báo hardware trong URDF với tag <ros2_control>
Cấu hình joint_state_broadcaster và diff_drive_controller
Test với mock hardware trước khi chạy trên phần cứng thật

ros2_control là framework mạnh mẽ — một khi bạn viết hardware interface cho robot của mình, tất cả controllers chuẩn của ROS 2 (PID, trajectory, diff drive, Ackermann) đều hoạt động ngay. Nếu bạn muốn tìm hiểu thêm về điều khiển robot bằng Python, hãy đọc bài Python điều khiển robot: từ GPIO đến ROS 2.