從零編寫控制器

本節編寫一個完整的雙關節位置目標控制器：沒有新命令時保持激活瞬間的位置，有合法目標後持續寫入目標。

1.示例 1：創建控制器包

在自己的學習工作區執行：

cd ~/ros2_control_humble_ws/src
ros2 pkg create learn_position_controller --build-type ament_cmake
cd learn_position_controller
mkdir -p src

1.1.完整 package.xml

<?xml version="1.0"?>
<package format="3">
  <name>learn_position_controller</name><version>0.0.1</version>
  <description>Two joint hold position learning controller</description>
  <maintainer email="learner@example.com">Learner</maintainer><license>Apache-2.0</license>
  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>controller_interface</depend><depend>pluginlib</depend><depend>rclcpp</depend>
  <depend>rclcpp_lifecycle</depend><depend>std_msgs</depend><depend>realtime_tools</depend>
  <export><build_type>ament_cmake</build_type></export>
</package>

1.2.完整 CMakeLists.txt

cmake_minimum_required(VERSION 3.16)
project(learn_position_controller)
set(CMAKE_CXX_STANDARD 17)
find_package(ament_cmake REQUIRED)
find_package(controller_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(std_msgs REQUIRED)
find_package(realtime_tools REQUIRED)
add_library(${PROJECT_NAME} SHARED src/hold_controller.cpp)
ament_target_dependencies(${PROJECT_NAME} controller_interface pluginlib rclcpp
  rclcpp_lifecycle std_msgs realtime_tools)
pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
install(TARGETS ${PROJECT_NAME} DESTINATION lib)
ament_package()

1.3.完整 controller_plugins.xml

<library path="learn_position_controller">
  <class name="learn_position_controller/HoldController"
         type="learn_position_controller::HoldController"
         base_class_type="controller_interface::ControllerInterface"/>
</library>

2.示例 2：編寫完整控制器

保存為 src/hold_controller.cpp：

#include <array>
#include <cmath>
#include <functional>
#include "controller_interface/controller_interface.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "realtime_tools/realtime_buffer.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
namespace learn_position_controller
{
struct Target {std::array<double, 2> q{0.0, 0.0}; bool valid{false};};
class HoldController : public controller_interface::ControllerInterface
{
public:
  controller_interface::CallbackReturn on_init() override
  {return controller_interface::CallbackReturn::SUCCESS;}
  controller_interface::InterfaceConfiguration command_interface_configuration() const override
  {return {controller_interface::interface_configuration_type::INDIVIDUAL,
      {"joint1/position", "joint2/position"}};}
  controller_interface::InterfaceConfiguration state_interface_configuration() const override
  {return {controller_interface::interface_configuration_type::INDIVIDUAL,
      {"joint1/position", "joint2/position"}};}
  controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override
  {
    target_.writeFromNonRT(Target{});
    subscription_ = get_node()->create_subscription<std_msgs::msg::Float64MultiArray>(
      "~/commands", 10, std::bind(&HoldController::receive, this, std::placeholders::_1));
    return controller_interface::CallbackReturn::SUCCESS;
  }
  controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override
  {
    if (command_interfaces_.size() != 2 || state_interfaces_.size() != 2) {
      return controller_interface::CallbackReturn::ERROR;
    }
    // 不假设框架借出的句柄顺序，按全名建立索引。
    for (std::size_t j = 0; j < 2; ++j) {
      const std::string name = "joint" + std::to_string(j + 1) + "/position";
      for (std::size_t k = 0; k < 2; ++k) {
        if (command_interfaces_[k].get_name() == name) {ci_[j] = k;}
        if (state_interfaces_[k].get_name() == name) {si_[j] = k;}
      }
      hold_[j] = state_interfaces_[si_[j]].get_value();
      if (!std::isfinite(hold_[j])) {return controller_interface::CallbackReturn::ERROR;}
    }
    target_.writeFromNonRT(Target{});
    return controller_interface::CallbackReturn::SUCCESS;
  }
  controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override
  {
    for (std::size_t j = 0; j < 2; ++j) {
      command_interfaces_[ci_[j]].set_value(state_interfaces_[si_[j]].get_value());
    }
    return controller_interface::CallbackReturn::SUCCESS;
  }
  controller_interface::return_type update(const rclcpp::Time &, const rclcpp::Duration &) override
  {
    const Target snapshot = *target_.readFromRT();
    for (std::size_t j = 0; j < 2; ++j) {
      command_interfaces_[ci_[j]].set_value(snapshot.valid ? snapshot.q[j] : hold_[j]);
    }
    return controller_interface::return_type::OK;
  }
private:
  void receive(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
  {
    if (msg->data.size() != 2) {return;}
    for (double q : msg->data) {
      if (!std::isfinite(q) || q < -1.0 || q > 1.0) {return;}
    }
    target_.writeFromNonRT(Target{{msg->data[0], msg->data[1]}, true});
  }
  std::array<std::size_t, 2> ci_{0, 1}, si_{0, 1};
  std::array<double, 2> hold_{0.0, 0.0};
  realtime_tools::RealtimeBuffer<Target> target_;
  rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr subscription_;
};
}
PLUGINLIB_EXPORT_CLASS(learn_position_controller::HoldController,
  controller_interface::ControllerInterface)

2.1.關鍵函數説明

command_interface_configuration() 聲明要寫的接口；state_interface_configuration() 聲明要讀的接口。二者返回 InterfaceConfiguration，本例選擇 INDIVIDUAL，並列出完整名稱。

on_configure() 創建訂閲並初始化目標快照。receive() 在非實時回調中驗證長度、有限值和範圍，再寫入緩衝。

on_activate() 根據句柄全名建立索引，並記錄當前反饋作為保持位置。這個索引工作只在激活時進行，update 中不再搜索字符串。

update() 讀取一次快照，然後設置借出的命令接口。它不直接打開串口，不發佈電機協議，也不重新獲取硬件資源。

2.2.為什麼還要檢查收到的數據

Float64MultiArray 只約定數據類型，不會替你限制數組長度、NaN 或關節範圍。若把非法浮點數一路傳下去，真實設備協議層可能產生錯誤轉換。

2.3.這個控制器實現了什麼控制律

它實現目標選擇與位置保持，位置閉環由後端承擔；沒有新增 PID。MiniSystem 用教學模型響應，Gazebo 使用仿真接口，真實驅動器則需要自己的位置跟蹤能力。

3.編譯並切換

停止舊實例後構建兩個自編包，再重啓 mini：

cd ~/ros2_control_humble_ws
colcon build --symlink-install --packages-select learn_control_hardware learn_position_controller
source install/setup.bash
ros2 launch learn_control_hardware mini.launch.py

終端 2：

source /opt/ros/humble/setup.bash
source ~/ros2_control_humble_ws/install/setup.bash

ros2 control load_controller learn_position_controller --set-state inactive
ros2 control switch_controllers --deactivate forward_position_controller \
  --activate learn_position_controller --strict
ros2 topic pub --once /learn_position_controller/commands \
  std_msgs/msg/Float64MultiArray "{data: [-0.2, 0.4]}"
ros2 topic echo /joint_states

3.1.運行結果與輸入檢查

發送合法目標後，反饋應逐漸收斂到 [-0.2, 0.4] 附近。對照 receive() 可見，長度不符、NaN 或超出範圍的目標會被拒絕，控制器繼續使用上一份合法目標。檢查反饋時按關節名稱匹配，不依賴 JointState 數組順序。

3.2.本示例需要注意的點

這是兩個固定名稱關節的入門控制器。擴展任意關節數量時，需要參數化 joints、預分配所有存儲，並繼續明確激活與命令過期策略。位置目標持續保持是本例的設計，不能自動套用到速度或力矩控制。

源碼對照：ros2_control/controller_interface/include/controller_interface/controller_interface.hpp、ros2_controllers/forward_command_controller/src/forward_controllers_base.cpp。

---

返回 Humble 總覽 · 上一篇：多接口與硬件類型 · 下一篇：管理器與資源管理源碼