Wiki

ros2_control整体工作过程

更新于 2023-12-30约 3 分钟阅读

先理解“目标、反馈、循环”，再记住管理器和插件的类名。

1.示例 1：普通程序中的控制周期

1.1.程序目标

模拟位置设备：读取上一次命令作用后的状态，更新本轮命令，再交给设备。保存为 /tmp/jazzy_cycle.cpp：

#include <chrono>
#include <cmath>
#include <iostream>
#include <thread>
int main()
{
  double feedback = 0.0;
  double applied = 0.0;
  const double target = 0.5;
  auto next = std::chrono::steady_clock::now();
  for (int cycle = 0; cycle < 100; ++cycle) {
    feedback += (1.0 - std::exp(-0.01 / 0.2)) * (applied - feedback); // read
    const double command = target; // update
    applied = command; // write
    if (cycle % 20 == 0) {
      std::cout << cycle << " feedback=" << feedback << " command=" << command << '\n';
    }
    next += std::chrono::milliseconds(10);
    std::this_thread::sleep_until(next);
  }
}

g++ -std=c++17 /tmp/jazzy_cycle.cpp -o /tmp/jazzy_cycle
/tmp/jazzy_cycle

1.2.运行输出与执行顺序

第一轮反馈仍为零，之后逐渐靠近 0.5。read 在本轮 write 之前，因此它不能读到尚未发生的设备响应。这里的一阶公式只表示教学模型，不含真实电机与碰撞。

2.把程序对应到框架

概念	对应职责
ControllerManager	管理控制器和调度
ResourceManager	管理硬件组件、接口和资源借用
硬件插件 read/write	将设备数据与框架接口连接
控制器 update	由目标与反馈计算接口命令
URDF	声明模型、硬件后端与接口
YAML、launch、spawner	提供配置并启动这些组件

3.示例 2：收到消息后发生什么

ROS 订阅回调收到目标
  → 保存最新完整目标
控制循环 read 获取反馈
  → update 读取目标并设置命令接口
  → write 下发目标
下一轮 read 获取新的反馈

消息回调和控制循环是不同执行路径。消息频率不等于 update_rate，回调也不应该直接承担整个设备控制循环。

3.1.本节需要注意的点

position 命令表示目标位置，state 表示反馈；真实设备可能延迟、跟踪失败或限幅。把命令直接当成反馈，会失去检查设备行为的依据。

旋转关节的位置和速度通常为 rad、rad/s；直线关节为 m、m/s。传输协议中的编码器数值、rpm 和电流要在设备适配层明确换算。

源码入口：ros2_control/controller_manager/src/ros2_control_node.cpp。找到控制线程和 Executor，观察 read、update、write 与 spin 各自运行在哪里。