從零編寫控制器

編寫雙關節位置保持控制器：激活時記錄當前位置，收到合法目標後持續寫入目標。

1.示例 1：創建包並填寫構建文件

cd ~/ros2_control_jazzy_ws/src
ros2 pkg create jazzy_position_controller --build-type ament_cmake
cd jazzy_position_controller
mkdir -p src

完整 package.xml：

<?xml version="1.0"?>
<package format="3">
  <name>jazzy_position_controller</name><version>0.1.0</version>
  <description>Jazzy position target 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>

完整 CMakeLists.txt：

cmake_minimum_required(VERSION 3.22)
project(jazzy_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/position_hold.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()

完整 controller_plugins.xml：

<library path="jazzy_position_controller">
  <class name="jazzy_position_controller/PositionHold"
    type="jazzy_position_controller::PositionHold"
    base_class_type="controller_interface::ControllerInterface"/>
</library>

2.示例 2：完整實現

保存 src/position_hold.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 jazzy_position_controller
{
struct Target {std::array<double, 2> q{}; bool valid{false};};
class PositionHold : 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{});
    sub_ = get_node()->create_subscription<std_msgs::msg::Float64MultiArray>(
      "~/commands", 10, std::bind(&PositionHold::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) {
      bool command_found = false, state_found = false;
      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; command_found = true;}
        if (state_interfaces_[k].get_name() == name) {si_[j] = k; state_found = true;}
      }
      if (!command_found || !state_found) {return controller_interface::CallbackReturn::ERROR;}
      const auto value = state_interfaces_[si_[j]].get_optional();
      if (!value || !std::isfinite(*value)) {return controller_interface::CallbackReturn::ERROR;}
      hold_[j] = *value;
    }
    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) {
      const auto value = state_interfaces_[si_[j]].get_optional();
      if (!value || !command_interfaces_[ci_[j]].set_value(*value)) {
        return controller_interface::CallbackReturn::ERROR;
      }
    }
    return controller_interface::CallbackReturn::SUCCESS;
  }
  controller_interface::return_type update(const rclcpp::Time &, const rclcpp::Duration &) override
  {
    const Target next = *target_.readFromRT();
    for (std::size_t j = 0; j < 2; ++j) {
      if (!command_interfaces_[ci_[j]].set_value(next.valid ? next.q[j] : hold_[j])) {
        return controller_interface::return_type::ERROR;
      }
    }
    return controller_interface::return_type::OK;
  }
private:
  void receive(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
  {
    if (msg->data.size() != 2) {return;}
    for (double value : msg->data) {
      if (!std::isfinite(value) || std::abs(value) > 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_{};
  realtime_tools::RealtimeBuffer<Target> target_;
  rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr sub_;
};
}
PLUGINLIB_EXPORT_CLASS(jazzy_position_controller::PositionHold,
  controller_interface::ControllerInterface)

2.1.關鍵函數説明

兩個 interface_configuration 函數分別聲明要讀、要寫的完整接口名。on_configure 創建訂閲；on_activate 按名字建立索引並檢查初始反饋；update 只取固定大小快照並寫接口。

get_optional 為空不能解引用，set_value 返回 false 不能假定寫入成功。接收回調先檢查數組長度、有限值和範圍，再更新目標。

位置保持是本控制器的明確設計，設備後端承擔位置跟蹤。它沒有新增 PID，也沒有為速度或力矩設備定義通用停止策略。

3.編譯與切換

停止舊實例後：

cd ~/ros2_control_jazzy_ws
colcon build --symlink-install --packages-select jazzy_control_hardware jazzy_position_controller
source install/setup.bash
ros2 launch jazzy_control_hardware two_joint.launch.py

另一終端：

source /opt/ros/jazzy/setup.bash
source ~/ros2_control_jazzy_ws/install/setup.bash

ros2 run controller_manager spawner position_hold --inactive \
  --param-file $(ros2 pkg prefix jazzy_control_hardware)/share/jazzy_control_hardware/config/controllers.yaml
ros2 control switch_controllers --deactivate forward_position_controller \
  --activate position_hold --strict
ros2 topic pub --once /position_hold/commands std_msgs/msg/Float64MultiArray \
  "{data: [-0.2, 0.4]}"

反饋應逐漸靠近 [-0.2,0.4]。非法目標被回調拒絕時，上一份合法目標繼續保持。狀態數組仍應按名稱匹配。

3.1.擴展前要考慮什麼

任意關節數量需要參數化、預分配和索引檢查；目標是否過期、激活後是否接受緩存目標，也應有明確策略。

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

---

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