Wiki

從零編寫模擬硬體外掛

更新於 2023-12-30約 4 分鐘閱讀

本文由簡體中文內容確定性轉換，並受版本化術語表保護。

本節用 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.關鍵函式說明

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啟動過程 · 下一篇：狀態介面與命令介面原始碼