Wiki

ros2_control整体工作过程

Updated 2023-12-30About 3 min read

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

本节先用一个普通程序看控制周期，不接 ROS、不接串口。先理解数据，再记类名。

1.示例 1：普通 main() 中模拟控制周期

1.1.Program Objectives

每 10 ms 执行一次：读反馈、计算命令、应用命令。目标位置为 0.5 rad。

把下面代码保存为 /tmp/control_cycle.cpp：

#include <chrono>
#include <cmath>
#include <iostream>
#include <thread>
int main()
{
  double position = 0.0;
  double command = 0.0;
  double applied = 0.0;
  const double goal = 0.5;
  const double dt = 0.01;
  auto next = std::chrono::steady_clock::now();
  for (int cycle = 0; cycle < 100; ++cycle) {
    // read：模拟读取设备在上一轮命令作用后的反馈。
    position += (1.0 - std::exp(-dt / 0.2)) * (applied - position);
    // update：这个控制器只转发目标位置。
    command = goal;
    // write：把本轮命令交给模拟设备。
    applied = command;
    if (cycle % 20 == 0) {
      std::cout << cycle << " q=" << position << " command=" << command << '\n';
    }
    next += std::chrono::milliseconds(10);
    std::this_thread::sleep_until(next);
  }
}

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

1.2.Run output and chronological order

输出应显示位置逐渐接近 0.5。第一轮位置仍是 0，因为第一次 read 发生在第一次 write 之前。具体打印的小数以运行结果为准。

第 0 轮 read：读取初始反馈 0
第 0 轮 update：命令变为 0.5
第 0 轮 write：设备接受 0.5
第 1 轮 read：读到变化后的反馈

1.3.为什么命令和反馈不是同一个变量

command 表示希望设备做到什么；position 表示实际读到了什么。真实设备有延迟、惯性、故障和限幅，因此二者可能长期不相等。直接把命令当成反馈，会掩盖设备未运动的问题。

这里的一阶公式是教学模型，不能证明真实机械臂的物理性能。

2.把变量对应到框架组件

本程序中的内容	ros2_control 中的对应部分
循环及周期安排	ros2_control_node / ControllerManager
管理硬件、接口和生命周期	ResourceManager
position 等反馈变量	硬件插件成员变量，经 StateInterface 导出
command 等目标变量	硬件插件成员变量，经 CommandInterface 导出
update 中的算法	ControllerInterface 的派生控制器
获取设备反馈、下发命令	硬件插件 read / write

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

ROS Executor 调用订阅回调
  → 回调保存最新目标
控制线程执行下一轮 update
  → 读取保存的目标
  → 修改命令接口
控制线程继续执行 write
  → 将命令交给设备

订阅回调与控制周期是两条执行路径。消息到达频率不等于控制周期频率；没有新消息时，控制器也可能继续使用已有目标。

3.1.Points to note in this example

ros2_control 是控制集成框架，不会替所有设备自动设计 PID。
position、velocity、effort 是接口语义；对应单位通常为 rad、rad/s、N·m，直线关节采用 m、m/s、N。
update_rate: 100 是调度目标，不能据此宣称每轮都精确耗时 10 ms。
RViz 是显示工具；Gazebo 才在后续章节引入物理引擎。

4.对应底层源码

从 ros2_control/controller_manager/src/ros2_control_node.cpp 的 main() 开始，找到控制线程和 cm->read()、cm->update()、cm->write()。本程序刻意保留相同顺序，后面再解释 ROS 时间与实际周期。