從零編寫模擬硬體外掛

本節把從零建立學習工作區的 GenericSystem 換成你自己編寫的雙關節模擬硬體。關節名稱和控制器 Topic 保持一致。

1.程式目標

實現一個具有 position 命令、position 與 velocity 狀態的硬體外掛。write 接受目標，下一輪 read 更新模擬反饋，響應時間常數為 0.2 秒。

2.示例 1：編寫標頭檔案

儲存為 include/learn_control_hardware/mini_system.hpp：

#ifndef LEARN_CONTROL_HARDWARE__MINI_SYSTEM_HPP_
#define LEARN_CONTROL_HARDWARE__MINI_SYSTEM_HPP_
#include <vector>
#include "hardware_interface/system_interface.hpp"
namespace learn_control_hardware
{
class MiniSystem : public hardware_interface::SystemInterface
{
public:
  hardware_interface::CallbackReturn on_init(
    const hardware_interface::HardwareInfo & info) override;
  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
  hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
  hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;
  hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override;
  hardware_interface::return_type read(const rclcpp::Time &, const rclcpp::Duration &) override;
  hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override;
protected:
  // 接口导出以后，不能再 resize 这些 vector，否则保存的元素地址可能失效。
  std::vector<double> position_, velocity_, command_, applied_;
};
}
#endif

3.示例 2：編寫完整實現

儲存為 src/mini_system.cpp：

#include "learn_control_hardware/mini_system.hpp"
#include <algorithm>
#include <cmath>
#include "pluginlib/class_list_macros.hpp"
namespace learn_control_hardware
{
hardware_interface::CallbackReturn MiniSystem::on_init(
  const hardware_interface::HardwareInfo & info)
{
  if (SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) {
    return hardware_interface::CallbackReturn::ERROR;
  }
  if (info_.joints.size() != 2) {return hardware_interface::CallbackReturn::ERROR;}
  for (const auto & joint : info_.joints) {
    if (joint.command_interfaces.size() != 1 ||
      joint.command_interfaces[0].name != "position" ||
      joint.state_interfaces.size() != 2 ||
      joint.state_interfaces[0].name != "position" ||
      joint.state_interfaces[1].name != "velocity") {
      return hardware_interface::CallbackReturn::ERROR;
    }
  }
  position_.assign(2, 0.0); velocity_.assign(2, 0.0);
  command_.assign(2, 0.0); applied_.assign(2, 0.0);
  return hardware_interface::CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> MiniSystem::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> result;
  for (std::size_t i = 0; i < position_.size(); ++i) {
    result.emplace_back(info_.joints[i].name, "position", &position_[i]);
    result.emplace_back(info_.joints[i].name, "velocity", &velocity_[i]);
  }
  return result;
}
std::vector<hardware_interface::CommandInterface> MiniSystem::export_command_interfaces()
{
  std::vector<hardware_interface::CommandInterface> result;
  for (std::size_t i = 0; i < command_.size(); ++i) {
    result.emplace_back(info_.joints[i].name, "position", &command_[i]);
  }
  return result;
}
hardware_interface::CallbackReturn MiniSystem::on_configure(const rclcpp_lifecycle::State &)
{
  std::fill(position_.begin(), position_.end(), 0.0);
  std::fill(velocity_.begin(), velocity_.end(), 0.0);
  std::copy(position_.begin(), position_.end(), command_.begin());
  std::copy(position_.begin(), position_.end(), applied_.begin());
  return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::CallbackReturn MiniSystem::on_activate(const rclcpp_lifecycle::State &)
{
  // 激活时对齐目标和反馈，避免继承上一次残留命令。
  std::copy(position_.begin(), position_.end(), command_.begin());
  std::copy(position_.begin(), position_.end(), applied_.begin());
  return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::CallbackReturn MiniSystem::on_deactivate(const rclcpp_lifecycle::State &)
{
  std::copy(position_.begin(), position_.end(), command_.begin());
  std::copy(position_.begin(), position_.end(), applied_.begin());
  std::fill(velocity_.begin(), velocity_.end(), 0.0);
  return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::return_type MiniSystem::read(
  const rclcpp::Time &, const rclcpp::Duration & period)
{
  const double dt = period.seconds();
  if (dt <= 0.0) {return hardware_interface::return_type::OK;}
  // 一阶响应：仅用于教学，不是具有质量、摩擦和碰撞的物理仿真。
  const double alpha = 1.0 - std::exp(-dt / 0.2);
  for (std::size_t i = 0; i < position_.size(); ++i) {
    const double step = alpha * (applied_[i] - position_[i]);
    velocity_[i] = step / dt;
    position_[i] += step;
  }
  return hardware_interface::return_type::OK;
}
hardware_interface::return_type MiniSystem::write(
  const rclcpp::Time &, const rclcpp::Duration &)
{
  for (std::size_t i = 0; i < command_.size(); ++i) {
    if (!std::isfinite(command_[i])) {return hardware_interface::return_type::ERROR;}
  }
  for (std::size_t i = 0; i < command_.size(); ++i) {
    applied_[i] = std::clamp(command_[i], -1.0, 1.0);
  }
  return hardware_interface::return_type::OK;
}
}
PLUGINLIB_EXPORT_CLASS(learn_control_hardware::MiniSystem, hardware_interface::SystemInterface)

3.1.關鍵函式說明

on_init(const HardwareInfo & info)：呼叫父類儲存解析後的配置，再驗證關節與介面。返回 CallbackReturn::SUCCESS 或 ERROR。此處一次性分配儲存，介面匯出後不再改變容量。

export_state_interfaces()：返回描述反饋儲存位置的控制代碼。&position_[i] 是成員元素地址，不能替換成區域性變數地址。

export_command_interfaces()：返回控制器將來可以寫入的控制代碼。控制器寫的是 command_，並沒有在這個函式中直接呼叫裝置。

read(time, period)：根據上一輪應用的目標推進教學模型。period.seconds() 是測得的週期，用於計算離散響應；返回 hardware_interface::return_type。

write(time, period)：先檢查所有值是否有限，再限幅並更新 applied_。先完整驗證，避免半組命令更新後才發現另一項無效。

3.2.為什麼保留 command_ 和 applied_ 兩份量

command_ 是控制器可寫儲存；applied_ 表示上一次 write 已交給裝置的目標。分開以後，read 處理上一輪命令的關係就清楚了。

4.示例 3：註冊外掛與構建

儲存為包根目錄 hardware_plugins.xml：

<library path="learn_control_hardware">
  <class name="learn_control_hardware/MiniSystem"
         type="learn_control_hardware::MiniSystem"
         base_class_type="hardware_interface::SystemInterface">
    <description>Two joint position learning system</description>
  </class>
</library>

用下面內容替換整個 CMakeLists.txt：

cmake_minimum_required(VERSION 3.16)
project(learn_control_hardware)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
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/mini_system.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 hardware_plugins.xml)
install(TARGETS ${PROJECT_NAME} DESTINATION lib)
install(DIRECTORY include/ DESTINATION include)
install(DIRECTORY urdf config launch DESTINATION share/${PROJECT_NAME})
ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})
ament_export_dependencies(hardware_interface pluginlib rclcpp rclcpp_lifecycle)
ament_package()

PLUGINLIB_EXPORT_CLASS 匯出 C++ 類；外掛 XML 提供查詢名稱；CMake 安裝庫並登記描述檔案。這三部分缺任何一個，都可能導致載入失敗。

5.示例 4：替換模型中的硬體後端

只修改 urdf/mini.urdf 的這一行：

<plugin>learn_control_hardware/MiniSystem</plugin>

它替換原來的 <plugin>mock_components/GenericSystem</plugin>，其餘關節和介面不變。

6.編譯執行

先停止舊 launch，再執行：

cd ~/ros2_control_humble_ws
colcon build --symlink-install --packages-select learn_control_hardware
source install/setup.bash

ros2 launch learn_control_hardware mini.launch.py

另一個已 source 的終端：

ros2 topic pub --once /forward_position_controller/commands \
  std_msgs/msg/Float64MultiArray "{data: [0.5, -0.3]}"
ros2 topic echo /joint_states

6.1.執行結果與檢查方法

啟動後先確認硬體和控制器處於 active，命令介面已 claimed。傳送 [0.5, -0.3] 後，按關節名稱檢查反饋：joint1 應逐漸接近 0.5 rad，joint2 應逐漸接近 -0.3 rad。模型存在一階響應延遲，不能要求反饋與目標瞬間相等。

6.2.本示例需要注意的點

這裡的 [-1, 1] 是這臺教學機器人的明確約定，不能原樣用於任意實物。read 中的響應公式不包含質量、接觸或電機模型；物理模擬在機械臂物理模擬完成。

對照官方實現：ros2_control_demos/example_1/hardware/rrbot.cpp。比較它的模擬方式、生命週期和外掛匯出，理解程式碼可以不同，但介面契約必須一致。

---

返回 Humble 總覽 · 上一篇：YAML與launch啟動過程 · 下一篇：狀態介面與命令介面原始碼