从零编写模拟硬件插件

本节用 Jazzy 的框架管理接口实现双关节模拟硬件。保持关节名称与上层控制器不变，只替换后端。

1.程序目标

read 按时间步长推进一阶响应，write 取框架命令并检查、限幅。接口由框架根据 URDF 创建，插件不手动返回指向成员数组的接口句柄。

2.示例 1：完整头文件

保存为 include/jazzy_control_hardware/two_joint_system.hpp：

#ifndef JAZZY_CONTROL_HARDWARE__TWO_JOINT_SYSTEM_HPP_
#define JAZZY_CONTROL_HARDWARE__TWO_JOINT_SYSTEM_HPP_
#include <array>
#include <string>
#include "hardware_interface/system_interface.hpp"
namespace jazzy_control_hardware
{
class TwoJointSystem : public hardware_interface::SystemInterface
{
public:
  hardware_interface::CallbackReturn on_init(
    const hardware_interface::HardwareComponentInterfaceParams & params) 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:
  std::array<std::string, 2> position_name_, velocity_name_;
  std::array<double, 2> q_{}, dq_{}, applied_{};
};
}
#endif

3.示例 2：完整实现

保存为 src/two_joint_system.cpp：

#include "jazzy_control_hardware/two_joint_system.hpp"
#include <algorithm>
#include <cmath>
#include "pluginlib/class_list_macros.hpp"
namespace jazzy_control_hardware
{
hardware_interface::CallbackReturn TwoJointSystem::on_init(
  const hardware_interface::HardwareComponentInterfaceParams & params)
{
  if (SystemInterface::on_init(params) != hardware_interface::CallbackReturn::SUCCESS) {
    return hardware_interface::CallbackReturn::ERROR;
  }
  if (info_.joints.size() != 2) {return hardware_interface::CallbackReturn::ERROR;}
  for (std::size_t i = 0; i < 2; ++i) {
    const auto & joint = info_.joints[i];
    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_name_[i] = joint.name + "/position";
    velocity_name_[i] = joint.name + "/velocity";
  }
  // 不手动导出指向 q_ 的句柄；由框架根据 URDF 创建接口。
  return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::CallbackReturn TwoJointSystem::on_configure(const rclcpp_lifecycle::State &)
{
  q_.fill(0.0); dq_.fill(0.0); applied_.fill(0.0);
  for (std::size_t i = 0; i < 2; ++i) {
    set_state(position_name_[i], q_[i]);
    set_state(velocity_name_[i], dq_[i]);
    set_command(position_name_[i], q_[i]);
  }
  return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::CallbackReturn TwoJointSystem::on_activate(const rclcpp_lifecycle::State &)
{
  for (std::size_t i = 0; i < 2; ++i) {
    q_[i] = get_state(position_name_[i]);
    if (!std::isfinite(q_[i])) {return hardware_interface::CallbackReturn::ERROR;}
    applied_[i] = q_[i]; set_command(position_name_[i], q_[i]);
  }
  return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::CallbackReturn TwoJointSystem::on_deactivate(const rclcpp_lifecycle::State &)
{
  applied_ = q_; dq_.fill(0.0);
  for (std::size_t i = 0; i < 2; ++i) {
    set_command(position_name_[i], q_[i]); set_state(velocity_name_[i], 0.0);
  }
  return hardware_interface::CallbackReturn::SUCCESS;
}
hardware_interface::return_type TwoJointSystem::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 < 2; ++i) {
    const double step = alpha * (applied_[i] - q_[i]);
    q_[i] += step; dq_[i] = step / dt;
    set_state(position_name_[i], q_[i]);
    set_state(velocity_name_[i], dq_[i]);
  }
  return hardware_interface::return_type::OK;
}
hardware_interface::return_type TwoJointSystem::write(
  const rclcpp::Time &, const rclcpp::Duration &)
{
  std::array<double, 2> next{};
  for (std::size_t i = 0; i < 2; ++i) {
    const double target = get_command(position_name_[i]);
    if (!std::isfinite(target)) {return hardware_interface::return_type::ERROR;}
    next[i] = std::clamp(target, -1.0, 1.0);
  }
  applied_ = next;
  return hardware_interface::return_type::OK;
}
}
PLUGINLIB_EXPORT_CLASS(jazzy_control_hardware::TwoJointSystem, hardware_interface::SystemInterface)

3.1.Key Function Description

on_init(params) 先调用父类，让框架保存和处理硬件描述，然后检查两关节与接口组合。参数类型为 HardwareComponentInterfaceParams，与所选 Jazzy 源码版本对应。

on_configure 初始化教学模型，并用 set_state、set_command 设置框架存储。真实设备的编码器反馈不能在配置时随意清零。

on_activate 读取已有位置，将初始目标与反馈对齐。on_deactivate 保持模型当前位置。

read(time, period) 使用实际 period 计算模型步长，再将状态写入接口。write 从 get_command 取得目标，先检查完整一组，再更新 applied_。

3.2.成员数组与框架接口是否相同

q_ 是模拟设备状态，applied_ 是设备已接收的目标。框架接口另有存储；set_state 把模型值发布到接口，get_command 从接口取出目标。它们不是自动共享同一块 double 地址。

4.示例 3：插件注册与构建

保存 hardware_plugins.xml：

<library path="jazzy_control_hardware">
  <class name="jazzy_control_hardware/TwoJointSystem"
    type="jazzy_control_hardware::TwoJointSystem"
    base_class_type="hardware_interface::SystemInterface"/>
</library>

替换完整 CMakeLists.txt：

cmake_minimum_required(VERSION 3.22)
project(jazzy_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/two_joint_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 launch config 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()

再把 two_joint.urdf 的 hardware/plugin 改为：

<plugin>jazzy_control_hardware/TwoJointSystem</plugin>

5.编译运行与预期现象

停止旧实例，再执行：

cd ~/ros2_control_jazzy_ws
colcon build --symlink-install --packages-select jazzy_control_hardware
source install/setup.bash

ros2 launch jazzy_control_hardware two_joint.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

反馈应逐渐接近 [0.5, -0.3]。0.2 秒是教学模型的时间常数，不是对真实设备的辨识结果；[-1,1] 是本模型明确采用的位置范围。

源码对照：ros2_control/hardware_interface/include/hardware_interface/hardware_component_interface.hpp、ros2_control_demos/example_1/hardware/rrbot.cpp。

---

返回 Jazzy 总览 · 上一篇：YAML与spawner启动过程 · 下一篇：状态接口与命令接口源码