从零编写控制器

编写双关节位置保持控制器：激活时记录当前位置，收到合法目标后持续写入目标。

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 总览 · 上一篇：多接口与硬件类型 · 下一篇：管理器与资源管理源码