Wiki

从零编写控制器

Updated 2023-12-30About 4 min read

Existing English translations were reused. 24 current source blocks remain in Simplified Chinese, without triggering paid translation.

本节编写一个完整的双关节位置目标控制器：没有新命令时保持激活瞬间的位置，有合法目标后持续写入目标。

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.Key Function Description

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

Terminal 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.Points to note in this example

这是两个固定名称关节的入门控制器。扩展任意关节数量时，需要参数化 joints、预分配所有存储，并继续明确激活与命令过期策略。位置目标持续保持是本例的设计，不能自动套用到速度或力矩控制。

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

返回 Humble 总览 · 上一篇：多接口与硬件类型 · 下一篇：管理器与资源管理源码