ros2_control整體工作過程

本節先用一個普通程序看控制週期，不接 ROS、不接串口。先理解數據，再記類名。

1.示例 1：普通 main() 中模擬控制週期

1.1.程序目標

每 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.運行輸出與時間順序

輸出應顯示位置逐漸接近 0.5。第一輪位置仍是 0，因為第一次 read 發生在第一次 write 之前。具體打印的小數以運行結果為準。

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

1.3.為什麼命令和反饋不是同一個變量

command 表示希望設備做到什麼；position 表示實際讀到了什麼。真實設備有延遲、慣性、故障和限幅，因此二者可能長期不相等。直接把命令當成反饋，會掩蓋設備未運動的問題。

這裏的一階公式是教學模型，不能證明真實機械臂的物理性能。

2.把變量對應到框架組件

3.示例 2：收到 ROS 消息以後發生什麼

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

訂閲回調與控制週期是兩條執行路徑。消息到達頻率不等於控制週期頻率；沒有新消息時，控制器也可能繼續使用已有目標。

3.1.本示例需要注意的點

1. ros2_control 是控制集成框架，不會替所有設備自動設計 PID。
2. position、velocity、effort 是接口語義；對應單位通常為 rad、rad/s、N·m，直線關節採用 m、m/s、N。
3. update_rate: 100 是調度目標，不能據此宣稱每輪都精確耗時 10 ms。
4. RViz 是顯示工具；Gazebo 才在後續章節引入物理引擎。

4.對應底層源碼

從 ros2_control/controller_manager/src/ros2_control_node.cpp 的 main() 開始，找到控制線程和 cm->read()、cm->update()、cm->write()。本程序刻意保留相同順序，後面再解釋 ROS 時間與實際週期。

---

返回 Humble 總覽 · 上一篇：環境準備與源碼地圖 · 下一篇：運行第一個RRBot實例