Viết Hardware Interface cho ros2_control

ros2_control là gì?

ros2_control là framework chuẩn trong ROS 2 để tách biệt control logic khỏi hardware communication. Thay vì mỗi robot viết controller riêng, ros2_control cung cấp kiến trúc chung:

Controller Manager
    ├── JointTrajectoryController (position control)
    ├── DiffDriveController (mobile robot)
    ├── ForwardCommandController (velocity/effort)
    └── ...

        ↕ State/Command Interfaces

Hardware Interface (bạn viết cái này!)
    ├── read() → đọc encoder, IMU, force sensor
    └── write() → gửi PWM, voltage, torque command

Bạn chỉ cần viết hardware interface -- phần giao tiếp với phần cứng thực. Tất cả controllers (PID, trajectory planning, differential drive) đã có sẵn và hoạt động với bất kỳ hardware interface nào.

Bài viết này hướng dẫn tạo custom hardware interface giao tiếp với motor driver qua serial port, từ đầu đến khi chạy được trên robot thật.

Kiến trúc ros2_control

Các thành phần chính

Controller Manager: Node chính, load/unload controllers và hardware interfaces
Controllers: Logic điều khiển (PID, trajectory, diff_drive...)
Hardware Interface: Giao tiếp phần cứng (bạn implement)
Resource Manager: Quản lý state/command interfaces giữa controllers và hardware

Interface types

ros2_control sử dụng 2 loại interface:

State Interface: Hardware → Controller. Ví dụ: joint1/position, joint1/velocity -- controller đọc giá trị từ sensor.
Command Interface: Controller → Hardware. Ví dụ: joint1/velocity, joint1/effort -- controller gửi lệnh đến actuator.

Hardware component types

Type	Mô tả	Ví dụ
SystemInterface	Nhiều joints, coupled hardware	Robot arm 6-DOF (1 bus điều khiển tất cả)
ActuatorInterface	1 joint, independent	Servo motor đơn lẻ
SensorInterface	Chỉ đọc, không điều khiển	Force/Torque sensor, IMU

Trong bài này, ta dùng SystemInterface vì đây là loại phổ biến nhất cho mobile robots và robot arms.

Step 1: Tạo ROS 2 package

# Tạo package
cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake \
    my_robot_hardware \
    --dependencies rclcpp hardware_interface pluginlib

# Cấu trúc
my_robot_hardware/
├── CMakeLists.txt
├── package.xml
├── include/my_robot_hardware/
│   └── serial_motor_interface.hpp
├── src/
│   └── serial_motor_interface.cpp
├── config/
│   └── controllers.yaml
├── urdf/
│   └── robot.urdf.xacro
└── my_robot_hardware.xml   # Plugin description

Step 2: Header file -- Declare interface

// include/my_robot_hardware/serial_motor_interface.hpp
#ifndef MY_ROBOT_HARDWARE__SERIAL_MOTOR_INTERFACE_HPP_
#define MY_ROBOT_HARDWARE__SERIAL_MOTOR_INTERFACE_HPP_

#include <string>
#include <vector>
#include <memory>

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

namespace my_robot_hardware
{

class SerialMotorInterface : public hardware_interface::SystemInterface
{
public:
  // Lifecycle callbacks
  hardware_interface::CallbackReturn on_init(
    const hardware_interface::HardwareInfo & info) override;

  hardware_interface::CallbackReturn on_configure(
    const rclcpp_lifecycle::State & previous_state) override;

  hardware_interface::CallbackReturn on_activate(
    const rclcpp_lifecycle::State & previous_state) override;

  hardware_interface::CallbackReturn on_deactivate(
    const rclcpp_lifecycle::State & previous_state) override;

  hardware_interface::CallbackReturn on_cleanup(
    const rclcpp_lifecycle::State & previous_state) override;

  // Export interfaces
  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

  // Read/Write -- called every control cycle
  hardware_interface::return_type read(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

  hardware_interface::return_type write(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

private:
  // Serial port
  int serial_fd_ = -1;
  std::string serial_port_;
  int baud_rate_;

  // Joint data
  std::vector<double> hw_commands_velocity_;   // From controller
  std::vector<double> hw_states_position_;     // To controller
  std::vector<double> hw_states_velocity_;     // To controller

  // Serial communication helpers
  bool open_serial(const std::string & port, int baud);
  void close_serial();
  bool send_command(const std::string & cmd);
  std::string read_response();
};

}  // namespace my_robot_hardware

#endif

Step 3: Implementation

// src/serial_motor_interface.cpp
#include "my_robot_hardware/serial_motor_interface.hpp"

#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <cstring>
#include <sstream>
#include <iomanip>

#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "pluginlib/class_list_macros.hpp"

namespace my_robot_hardware
{

hardware_interface::CallbackReturn SerialMotorInterface::on_init(
  const hardware_interface::HardwareInfo & info)
{
  // Call parent init
  if (hardware_interface::SystemInterface::on_init(info) !=
      hardware_interface::CallbackReturn::SUCCESS)
  {
    return hardware_interface::CallbackReturn::ERROR;
  }

  // Read parameters from URDF
  serial_port_ = info_.hardware_parameters.at("serial_port");
  baud_rate_ = std::stoi(info_.hardware_parameters.at("baud_rate"));

  // Initialize vectors
  size_t num_joints = info_.joints.size();
  hw_commands_velocity_.resize(num_joints, 0.0);
  hw_states_position_.resize(num_joints, 0.0);
  hw_states_velocity_.resize(num_joints, 0.0);

  RCLCPP_INFO(rclcpp::get_logger("SerialMotorInterface"),
    "Initialized with %zu joints on %s @ %d baud",
    num_joints, serial_port_.c_str(), baud_rate_);

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn SerialMotorInterface::on_configure(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // Open serial port
  if (!open_serial(serial_port_, baud_rate_)) {
    RCLCPP_ERROR(rclcpp::get_logger("SerialMotorInterface"),
      "Failed to open serial port %s", serial_port_.c_str());
    return hardware_interface::CallbackReturn::ERROR;
  }

  RCLCPP_INFO(rclcpp::get_logger("SerialMotorInterface"),
    "Serial port %s opened successfully", serial_port_.c_str());

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn SerialMotorInterface::on_activate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // Reset commands and states
  for (size_t i = 0; i < hw_commands_velocity_.size(); i++) {
    hw_commands_velocity_[i] = 0.0;
    hw_states_position_[i] = 0.0;
    hw_states_velocity_[i] = 0.0;
  }

  // Send enable command to motor driver
  send_command("ENABLE\n");

  RCLCPP_INFO(rclcpp::get_logger("SerialMotorInterface"), "Activated!");
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn SerialMotorInterface::on_deactivate(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  // Stop all motors
  for (size_t i = 0; i < hw_commands_velocity_.size(); i++) {
    hw_commands_velocity_[i] = 0.0;
  }
  send_command("DISABLE\n");

  RCLCPP_INFO(rclcpp::get_logger("SerialMotorInterface"), "Deactivated!");
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn SerialMotorInterface::on_cleanup(
  const rclcpp_lifecycle::State & /*previous_state*/)
{
  close_serial();
  return hardware_interface::CallbackReturn::SUCCESS;
}

// Export state interfaces -- controller reads these
std::vector<hardware_interface::StateInterface>
SerialMotorInterface::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> interfaces;

  for (size_t i = 0; i < info_.joints.size(); i++) {
    interfaces.emplace_back(
      info_.joints[i].name, hardware_interface::HW_IF_POSITION,
      &hw_states_position_[i]);
    interfaces.emplace_back(
      info_.joints[i].name, hardware_interface::HW_IF_VELOCITY,
      &hw_states_velocity_[i]);
  }

  return interfaces;
}

// Export command interfaces -- controller writes to these
std::vector<hardware_interface::CommandInterface>
SerialMotorInterface::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> interfaces;

  for (size_t i = 0; i < info_.joints.size(); i++) {
    interfaces.emplace_back(
      info_.joints[i].name, hardware_interface::HW_IF_VELOCITY,
      &hw_commands_velocity_[i]);
  }

  return interfaces;
}

// READ -- Called every control cycle (~100 Hz)
// Read encoder data from motor driver via serial
hardware_interface::return_type SerialMotorInterface::read(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
  // Send read command
  send_command("READ\n");

  // Parse response: "pos0,vel0,pos1,vel1\n"
  std::string response = read_response();
  if (response.empty()) {
    return hardware_interface::return_type::ERROR;
  }

  std::istringstream ss(response);
  std::string token;
  size_t idx = 0;

  while (std::getline(ss, token, ',') && idx < hw_states_position_.size()) {
    hw_states_position_[idx] = std::stod(token);
    if (std::getline(ss, token, ',')) {
      hw_states_velocity_[idx] = std::stod(token);
    }
    idx++;
  }

  return hardware_interface::return_type::OK;
}

// WRITE -- Called every control cycle (~100 Hz)
// Send velocity commands to motor driver via serial
hardware_interface::return_type SerialMotorInterface::write(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
  // Build command: "VEL 1.5,-0.8\n"
  std::ostringstream cmd;
  cmd << "VEL ";
  for (size_t i = 0; i < hw_commands_velocity_.size(); i++) {
    if (i > 0) cmd << ",";
    cmd << std::fixed << std::setprecision(4) << hw_commands_velocity_[i];
  }
  cmd << "\n";

  send_command(cmd.str());
  return hardware_interface::return_type::OK;
}

// Serial helpers
bool SerialMotorInterface::open_serial(const std::string & port, int baud)
{
  serial_fd_ = open(port.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
  if (serial_fd_ < 0) return false;

  struct termios tty;
  memset(&tty, 0, sizeof(tty));
  tcgetattr(serial_fd_, &tty);

  speed_t speed;
  switch (baud) {
    case 9600: speed = B9600; break;
    case 115200: speed = B115200; break;
    case 500000: speed = B500000; break;
    default: speed = B115200;
  }

  cfsetispeed(&tty, speed);
  cfsetospeed(&tty, speed);

  tty.c_cflag |= (CLOCAL | CREAD);
  tty.c_cflag &= ~PARENB;
  tty.c_cflag &= ~CSTOPB;
  tty.c_cflag &= ~CSIZE;
  tty.c_cflag |= CS8;
  tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
  tty.c_oflag &= ~OPOST;

  tty.c_cc[VMIN] = 0;
  tty.c_cc[VTIME] = 1;  // 100ms timeout

  tcsetattr(serial_fd_, TCSANOW, &tty);
  tcflush(serial_fd_, TCIOFLUSH);

  return true;
}

void SerialMotorInterface::close_serial()
{
  if (serial_fd_ >= 0) {
    close(serial_fd_);
    serial_fd_ = -1;
  }
}

bool SerialMotorInterface::send_command(const std::string & cmd)
{
  if (serial_fd_ < 0) return false;
  return ::write(serial_fd_, cmd.c_str(), cmd.size()) > 0;
}

std::string SerialMotorInterface::read_response()
{
  if (serial_fd_ < 0) return "";

  char buf[256];
  std::string result;

  // Read until newline or timeout
  int attempts = 0;
  while (attempts < 10) {
    int n = ::read(serial_fd_, buf, sizeof(buf) - 1);
    if (n > 0) {
      buf[n] = '\0';
      result += buf;
      if (result.find('\n') != std::string::npos) break;
    }
    attempts++;
    usleep(1000);  // 1ms
  }

  return result;
}

}  // namespace my_robot_hardware

// Register plugin
PLUGINLIB_EXPORT_CLASS(
  my_robot_hardware::SerialMotorInterface,
  hardware_interface::SystemInterface)

Step 4: URDF ros2_control tag

Đây là phần kết nối URDF với hardware interface:

<!-- urdf/robot.urdf.xacro -->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="my_robot">

  <!-- Robot links and joints (omitted for brevity) -->
  <link name="base_link"/>
  <link name="left_wheel"/>
  <link name="right_wheel"/>

  <joint name="left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel"/>
    <axis xyz="0 0 1"/>
  </joint>

  <joint name="right_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel"/>
    <axis xyz="0 0 1"/>
  </joint>

  <!-- ros2_control hardware interface -->
  <ros2_control name="MyRobotHardware" type="system">
    <hardware>
      <plugin>my_robot_hardware/SerialMotorInterface</plugin>
      <param name="serial_port">/dev/ttyUSB0</param>
      <param name="baud_rate">115200</param>
    </hardware>

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

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

Step 5: Controller Manager config

# config/controllers.yaml
controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    diff_drive_controller:
      type: diff_drive_controller/DiffDriveController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

diff_drive_controller:
  ros__parameters:
    left_wheel_names: ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]
    wheel_separation: 0.34       # meters
    wheel_radius: 0.05           # meters

    # Odometry
    publish_rate: 50.0
    odom_frame_id: odom
    base_frame_id: base_link

    # Velocity limits
    linear.x.has_velocity_limits: true
    linear.x.max_velocity: 1.0
    angular.z.has_velocity_limits: true
    angular.z.max_velocity: 2.0

    # PID (optional, for closed-loop velocity control)
    # left_wheel_joint.p: 10.0
    # left_wheel_joint.i: 5.0
    # left_wheel_joint.d: 0.1

Step 6: Build và Plugin registration

<!-- my_robot_hardware.xml -- Plugin description -->
<library path="my_robot_hardware">
  <class name="my_robot_hardware/SerialMotorInterface"
         type="my_robot_hardware::SerialMotorInterface"
         base_class_type="hardware_interface::SystemInterface">
    <description>
      Custom hardware interface for serial motor driver communication.
    </description>
  </class>
</library>

# CMakeLists.txt (relevant parts)
cmake_minimum_required(VERSION 3.8)
project(my_robot_hardware)

find_package(ament_cmake REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)

add_library(${PROJECT_NAME} SHARED
  src/serial_motor_interface.cpp
)

target_include_directories(${PROJECT_NAME} PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)

ament_target_dependencies(${PROJECT_NAME}
  hardware_interface
  pluginlib
  rclcpp
  rclcpp_lifecycle
)

pluginlib_export_plugin_description_file(
  hardware_interface my_robot_hardware.xml)

install(TARGETS ${PROJECT_NAME}
  DESTINATION lib
)

install(DIRECTORY include/
  DESTINATION include
)

install(DIRECTORY config urdf
  DESTINATION share/${PROJECT_NAME}
)

ament_package()

# Build
cd ~/ros2_ws
colcon build --packages-select my_robot_hardware
source install/setup.bash

Step 7: Testing với Mock Hardware

Trước khi chạy trên robot thật, test với mock hardware (không cần serial port):

<!-- urdf/robot_mock.urdf.xacro -->
<ros2_control name="MyRobotMock" type="system">
  <hardware>
    <plugin>mock_components/GenericSystem</plugin>
    <param name="mock_sensor_commands">false</param>
    <param name="state_following_offset">0.0</param>
  </hardware>

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

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

# Launch với mock hardware
ros2 launch my_robot_bringup robot.launch.py use_mock:=true

# Trong terminal khác -- kiểm tra controller
ros2 control list_controllers
# Expected output:
# diff_drive_controller [diff_drive_controller/DiffDriveController] active
# joint_state_broadcaster [joint_state_broadcaster/JointStateBroadcaster] active

# Test diff_drive
ros2 topic pub /diff_drive_controller/cmd_vel_unstamped geometry_msgs/msg/Twist \
  "{linear: {x: 0.2}, angular: {z: 0.0}}"

# Kiểm tra odometry
ros2 topic echo /diff_drive_controller/odom --once

CAN bus thay vì Serial

Với robot công nghiệp, CAN bus phổ biến hơn serial. Thay đổi chính:

// Thay serial bằng SocketCAN
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>

// Open CAN
int can_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
struct ifreq ifr;
strcpy(ifr.ifr_name, "can0");
ioctl(can_fd, SIOCGIFINDEX, &ifr);
struct sockaddr_can addr;
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
bind(can_fd, (struct sockaddr *)&addr, sizeof(addr));

// Send CAN frame
struct can_frame frame;
frame.can_id = 0x201;         // Motor 1 velocity command
frame.can_dlc = 4;            // 4 bytes
int32_t vel_cmd = (int32_t)(velocity * 1000);  // mm/s
memcpy(frame.data, &vel_cmd, 4);
::write(can_fd, &frame, sizeof(frame));

Checklist trước khi chạy trên robot thật

Test với mock hardware thành công
Serial/CAN communication verified (dùng minicom hoặc candump)
Emergency stop hoạt động (deactivate stops all motors)
Velocity limits configured trong URDF và controller YAML
Encoder direction đúng (forward = positive)
Wheel separation và radius đo chính xác
udev rules cho serial port (/dev/ttyUSB0 → /dev/my_robot)