从零编写模拟硬件插件

本节把从零创建学习工作区的 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启动过程 · 下一篇：状态接口与命令接口源码