实时控制学习2

1、 Wait Set

Wait Set = Executor 用来统一等待多个 ROS2 事件的机制。

你可以类比前面 Linux 学过的：

复制代码

poll / epoll

虽然实现和语义不是完全一样。

概念上都是：

复制代码

我有很多事件源
↓
不要一个个死循环检查
↓
统一等谁 ready

Linux：

复制代码

socket1
socket2
timerfd
eventfd
↓
epoll_wait()

ROS2：

复制代码

subscription
timer
service
client
↓
Executor / Wait Set

都是：

事件驱动。

但不要说：

复制代码

ROS2 Executor = epoll

它们不是同一个东西。

只是思维方式相似。

比如：

复制代码

/mocap/human_pose

来了一条消息。

大概：

复制代码

DDS收到
↓
RMW层标记subscription ready
↓
Executor从等待状态醒来
↓
选择这个subscription
↓
执行callback

所以中间可能存在：

复制代码

Executor scheduling delay

Executor 和 worker thread 怎么配合？

很常见结构：

复制代码

Executor callback
↓
快速接收数据
↓
push到queue
↓
立即返回

Worker Thread
↓
从queue取
↓
做重计算

这样 Executor 不容易被重任务堵住。

`spin_some()`

概念上：

复制代码

spin()
= 一直运行Executor

而：

复制代码

spin_some()

可以理解为：

处理当前一批 ready 的 callback，然后返回。

它适合某些：

复制代码

自定义主循环

场景。

但不要为了"可控"就随便自己手写复杂 spin loop。

`spin_once()`

简单理解：

处理一次 Executor 工作机会，然后返回。

这种常见于：

复制代码

集成其他event loop
测试
自定义主循环

但普通 Node：

复制代码

rclcpp::spin()

最简单。

Executor 线程到底有几个？

取决于 Executor 类型。

复制代码

SingleThreadedExecutor
→ 通常一个执行线程

MultiThreadedExecutor
→ 多个worker线程

但注意：

DDS 自己还可能有其他内部线程。

所以：

复制代码

ROS2进程只有一个Executor线程

不代表：

复制代码

整个进程只有一个线程

拿到 ROS2 工程时怎么检查 Executor？

你以后可以按这个顺序：

复制代码

① 找 main()
↓
② 看用了什么 Executor
↓
③ 看 add_node / spin
↓
④ 找 callback
↓
⑤ 看哪些callback可能阻塞
↓
⑥ 看有没有自定义thread
↓
⑦ 看共享数据同步

这是非常实用的源码阅读习惯。

2、MutuallyExclusive

名字拆开：

复制代码

Mutually
= 相互

Exclusive
= 排斥

也就是：

同一个 group 里的 callback 同一时间只允许一个执行。

例如：

复制代码

Group 1: MutuallyExclusive
├── Subscription A
├── Service B
└── Timer C

即使：

复制代码

MultiThreadedExecutor
有 8 个线程

A、B、C 也不会同时执行。

它们仍然是：

复制代码

A
↓
B
↓
C

串行。

最大好处：

复制代码

减少共享数据竞争

比如：

复制代码

Subscription callback
修改 current_state_

Service callback
也修改 current_state_

放在同一个：

复制代码

MutuallyExclusive Group

可以保证：

复制代码

不会同时操作 current_state_

这比到处：

复制代码

mutex.lock()
mutex.unlock()

有时候更简单。

3、Reentrant

简单理解：

这个 group 里的 callback 允许并发执行。

例如：

复制代码

Group 2: Reentrant
├── Subscription X
├── Subscription Y
└── Timer Z

在 MultiThreadedExecutor 下，

可能：

复制代码

Thread1 → X
Thread2 → Y
Thread3 → Z

同时执行。

Reentrant 更严格一点的含义是：

同一个 callback 逻辑也应该能够在尚未结束时，再次被进入。

比如某个 Subscription 消息来的很快：

复制代码

Callback instance 1
还没结束

新消息又来了，

如果调度条件允许：

复制代码

Callback instance 2

可能同时执行。

所以代码必须真的能承受：

复制代码

并发进入

4、Callback Group

所以你可以把它理解成：

复制代码

业务层并发约束

不是：

复制代码

纯性能优化开关

你不是为了：

复制代码

"线程越多越快"

而是表达：

复制代码

哪些操作互相冲突
哪些操作彼此独立

和 mutex 有什么区别？

mutex：

复制代码

保护具体共享资源

Callback Group：

复制代码

控制 callback 是否同时执行

例如：

复制代码

Group A MutuallyExclusive

让两个 callback 不并行。

但如果：

复制代码

另一个 worker thread

访问同一资源，

Callback Group 管不到。

这时仍然需要：

复制代码

mutex / atomic / lock-free

什么时候用 Callback Group，什么时候用 mutex？

可以这样理解。

如果两个 callback：

复制代码

本来就不应该同时执行

可以考虑：

复制代码

MutuallyExclusive Group

如果：

复制代码

允许并行
但只在某一小段共享数据上冲突

更适合：

复制代码

mutex

否则整个 callback 全部串行，

可能损失太多并发能力。

不要以为：

复制代码

command callback
放 MutuallyExclusive group

就能保护：

复制代码

RT thread

因为 RT thread 是 Executor 外部线程。RT thread 就是要求"必须在规定时间内完成"的线程。它不是跑得更快，而是"更可预测、更守时"

所以：

复制代码

Callback Group
= ROS2 Executor并发控制

不是整个进程的全局线程同步机制。

CPU-bound 和 I/O-bound

CPU-bound

程序大部分时间在算，CPU 跑满。

例如：

复制代码

GMR IK
图像处理
点云计算
矩阵运算

主要消耗：

复制代码

CPU计算

这种任务线程太多：

复制代码

很容易把CPU打满

I/O-bound

程序大部分时间在等，CPU 闲着。

例如：

复制代码

等待socket
等待磁盘
等待设备

大量时间可能：

复制代码

在阻塞等待

这类任务适当更多线程可能有价值。

但 ROS2 callback 里长期阻塞 I/O 本身通常就该谨慎。

怎么优化共享状态？

常见思路：

复制代码

减少共享数据

缩小mutex临界区

copy snapshot

atomic counter

producer-consumer queue

double buffer

而不是一上来：

复制代码

全局一把大锁

copy-under-lock

比如共享：

复制代码

latest_command_

可以：

复制代码

Command local;

{
    std::lock_guard<std::mutex> lock(mutex_);
    local = latest_command_;
}

// 锁已经释放
process(local);

这样：

复制代码

锁只保护复制

重计算：

复制代码

在锁外完成

能显著减少竞争。这和实时线程也非常重要

比如：

复制代码

ROS2 callback
↓
更新 latest command

实时线程：

复制代码

读取 command

不要让 RT thread：

复制代码

长期等一个被ROS2 callback持有的mutex

更倾向：

复制代码

短锁
snapshot
RealtimeBuffer

来降低阻塞。

Executor thread 和 RT thread 要区分

例如：

复制代码

MultiThreadedExecutor
4 workers

这些通常仍是：

复制代码

普通ROS2线程

而你的：

复制代码

EtherCAT 1kHz RT Thread

可以是：

复制代码

独立线程
CPU affinity
SCHED_FIFO

所以架构：

复制代码

ROS2 Executor Threads
↓
非实时域

Realtime Buffer

RT Control Thread
↓
实时域

不要把：

复制代码

MultiThreadedExecutor

理解成：

复制代码

自动实时线程池

怎么判断是否需要 MultiThreadedExecutor？

问这几个问题：

复制代码

① 有没有一个callback明显拖住其他callback？

② 哪些callback之间真的可以安全并行？

③ CPU是否有余量？

④ 是否存在大量锁竞争？

⑤ 顺序是否重要？

如果没有明确并行需求：

复制代码

SingleThreadedExecutor

完全可以继续用。

5、on_configure

通常负责：

复制代码

读取参数
打开设备
初始化资源
检查配置
建立连接
分配buffer

例如：

复制代码

unconfigured
↓
on_configure()
↓
inactive

注意：

配置完成后，设备不一定已经开始输出控制。

它只是：

复制代码

"准备好了"

6、on_activate

例如：

复制代码

inactive
↓
on_activate()
↓
active

这一步才真正：

复制代码

启动周期线程
开始采集
开始 publish
使能控制器
允许输出

也就是：

从"准备好"切换到"真正工作"。

7、on_deactivate

停用、取消激活。在 ROS 2 里，它最常见的场景是生命周期节点（Lifecycle Node）和 ros2_control 控制器，表示让一个正在运行的东西停止工作，但不销毁。

复制代码

active
↓
on_deactivate()
↓
inactive

通常负责：

复制代码

停止周期输出
停止采集
让设备进入安全状态
停止发布活动数据

但可以保留：

复制代码

已打开的设备
已加载的配置
已分配的资源

所以之后还能：

复制代码

再次 activate

而不用全部重新初始化。

deactivate 可以做什么？

比如：

复制代码

停止 receive thread
停止 publish

但：

复制代码

socket可以先保留
配置保留

以后：

复制代码

activate

再启动。cleanup 可以真正释放

比如：

复制代码

join thread
close socket
clear queue
destroy MotionInput

然后回：

复制代码

unconfigured

这样资源生命周期非常明确。

8、on_cleanup

复制代码

inactive
↓
on_cleanup()
↓
unconfigured

这时通常：

复制代码

关闭socket
关闭设备
释放buffer
清理内部状态

回到：

复制代码

像刚创建时那样

Created
↓
Configured
↓
Running
↓
Stopped
↓
Released

只是 ROS2 给你定义了一套统一生命周期语义。

9、activate

复制代码

inactive
↓
activate

做：

复制代码

启动 receive thread
开始解析帧
开始 publish /mocap/human_pose

于是进入：

复制代码

active

这就比：

复制代码

构造函数一上来直接收包

清晰很多。

这和 RAII 和Lifecycle

RAII 负责：

对象生命周期内，资源最终一定正确释放。

Lifecycle 负责：

运行过程中，资源在哪个状态被创建、启动、停止、释放。

所以：

复制代码

RAII
= C++资源安全

Lifecycle
= 系统运行状态管理

两者是互补的。

10、Lifecycle transition

状态之间的切换叫：

复制代码

transition

比如：

复制代码

configure
activate
deactivate
cleanup
shutdown

每个 transition 会触发对应 callback。

transition callback

比如：

复制代码

on_configure()

只负责：

复制代码

配置阶段资源

不要里面又：

复制代码

启动控制线程
开始运动
发控制命令

否则：

复制代码

configure

和：

复制代码

activate

语义就乱了。

Lifecycle 和 systemd

systemd 看：

复制代码

进程服务生命周期

比如：

复制代码

started
running
failed

Lifecycle 看：

复制代码

ROS2 Node内部业务状态

比如：

复制代码

unconfigured
inactive
active

所以：

复制代码

systemd
↓
进程层

Lifecycle
↓
ROS2功能层

退出：

复制代码

on_deactivate()
↓
stop_requested = true
↓
join receive thread
↓
确认不再访问socket

之后 cleanup：

复制代码

close socket
destroy source

这就很安全。

复制代码

on_init
→ 读取硬件描述

on_configure
→ 打开EtherCAT / 初始化设备

on_activate
→ 允许read/write周期运行

read
→ 读状态

write
→ 写命令

on_deactivate
→ 停止输出/安全关闭

Diagnostics = 把系统健康状态结构化地发布出来。

不是靠：

复制代码

printf
RCLCPP_INFO
人工翻日志

而是让系统持续产生：

复制代码

OK
WARN
ERROR
STALE

这样的健康状态。

复制代码

OK
WARN
ERROR
STALE

简单理解：

复制代码

OK
= 正常

WARN
= 还能运行，但已经有异常趋势

ERROR
= 功能已经明显异常

STALE
= 很久没有更新，状态可能已经过期

Diagnostics 和日志

日志：

复制代码

10:00:01 disconnected
10:00:02 reconnecting
10:00:03 reconnect failed

Diagnostics：

复制代码

Current Status:
ERROR

connected = false
retry_count = 3
last_error = "connection timeout"

所以：

复制代码

日志
= 过程记录

Diagnostics
= 当前健康摘要

两者应该一起用。

Safety 阈值和 Diagnostics 阈值

例如：

复制代码

Diagnostics:
队列使用率 > 80%
→ WARN

这只是健康告警。

而：

复制代码

Safety:
command timeout > 100ms
→ safe stop

这是控制动作。

所以：

复制代码

Diagnostics
= 告诉你系统状态

Safety
= 决定系统怎么保护自己

不要混在一起。

复制代码

Lifecycle
= 逻辑状态
Diagnostics
= 健康状态

两者组合特别有价值。

systemd：

复制代码

进程活着吗？

Lifecycle：

复制代码

Node处于什么运行阶段？

Diagnostics：

复制代码

业务健康吗？

可以理解：

复制代码

systemd
↓
Process Alive

Lifecycle
↓
Node State

Diagnostics
↓
System Health

11、ros2_control

ros2_control = ROS2 里用于统一"控制器"和"真实/仿真硬件"的框架。

它解决的问题是：

复制代码

上层控制器
不应该直接知道
底层电机/总线细节

所以会分层。

最核心关系：

复制代码

Controller
↓
Command Interface
↓
Hardware Interface
↓
真实硬件

状态反过来：

复制代码

真实硬件
↓
Hardware Interface
↓
State Interface
↓
Controller / broadcaster

controller_manager
hardware_interface
controller
joint_state_broadcaster
command/state interface

分别是：

复制代码

controller_manager
= 管理控制器和硬件生命周期

hardware_interface
= ROS2和真实硬件之间的适配层

controller
= 根据状态生成命令

joint_state_broadcaster
= 把硬件状态发布成 /joint_states

command/state interface
= 控制器和硬件之间交换数据的接口

                ROS2
                  │
        joint_trajectory_controller
                  │
          command interfaces
                  │
          controller_manager
                  │
          Hardware Interface
                  │
              EtherCAT
                  │
               Motors

状态反过来：

复制代码

Motors
↓
EtherCAT
↓
Hardware Interface
↓
state interfaces
↓
joint_state_broadcaster
↓
/joint_states

这两条链一定要记住。

复制代码

Controller
↓
标准接口
↓
Hardware Interface
↓
厂商SDK / EtherCAT

这样上层控制器不需要知道：

复制代码

PDO
Slave ID
CANopen object
EtherCAT驱动细节

Hardware Interface

把你的底层硬件驱动，包装成 ros2_control 能认识的标准接口。

例如你底层已经有：

复制代码

EtherCAT Master
↓
read_motor_position()
write_motor_torque()

你要封装成：

复制代码

Hardware Interface

给 ros2_control 使用。

effort control

在 ROS 2 里，effort 是"力/力矩"的标准叫法，effort control 就是直接控制关节输出力矩。effort control 不直接命令关节转到哪个角度，而是命令它"用多大力气"去转。关节实际怎么动，由力矩和外部环境共同决定。

如果硬件支持 torque：

复制代码

State:
position
velocity
effort

Command：

复制代码

effort

控制器可以算：

复制代码

τ = ...

然后：

复制代码

effort command
↓
write()
↓
EtherCAT

这种更接近动力学控制。

controller_manager

controller_manager = ros2_control 的核心管理器。

它负责：

复制代码

加载硬件
加载控制器
配置控制器
激活控制器
切换控制器
执行控制循环

所以你之前看到：

复制代码

/controller_manager

它不是一个普通业务节点，它是整个 ros2_control 系统的核心。

joint_state_broadcaster

它也是一种 controller/broadcaster。但它不输出电机命令。

它做：

复制代码

读取 state interfaces
↓
发布 /joint_states

所以：

复制代码

Hardware Interface
↓
position/velocity/effort
↓
joint_state_broadcaster
↓
/joint_states

电机编码器
↓
EtherCAT
↓
Hardware Interface read()
↓
State Interfaces
↓
joint_state_broadcaster
↓
/joint_states
↓
robot_state_publisher
↓
/tf
↓
RViz

现在你之前所有知识已经彻底串起来了。

joint_trajectory_controller

上层给它：

复制代码

JointTrajectory

里面有：

复制代码

joint_names
positions
velocities
time_from_start

然后它按照轨迹：

复制代码

不断生成关节命令

再写到 command interface。

不是：

复制代码

JointTrajectory
↓
EtherCAT

而是：

复制代码

JointTrajectory
↓
joint_trajectory_controller
↓
command interface
↓
Hardware Interface
↓
EtherCAT

中间有 controller。

forward_command_controller

它比 trajectory controller 简单。

可以理解：

上层给什么 command，它就直接往对应 command interface 写。

比如：

复制代码

position command

直接：

复制代码

forward_command_controller
↓
Hardware Interface

适合：

复制代码

简单测试
上层已经算好目标

Hardware Interface 生命周期

复制代码

on_init()
↓
on_configure()
↓
on_activate()
↓
read()/write()
↓
on_deactivate()

on_init()

一般做：

复制代码

读取 hardware info
解析 joint/interface 配置
初始化成员变量

不要马上：

复制代码

使能电机

on_configure()

适合做：

复制代码

打开 EtherCAT master
扫描 slave
检查设备
配置 PDO
分配 buffer

如果失败：

复制代码

configure失败

不要进入 active。

on_activate()

真正：

复制代码

允许周期控制
同步初始状态
使能输出
启动RT相关资源

进入工作状态。

read()

周期性：

复制代码

EtherCAT receive
↓
解析PDO
↓
position/velocity/effort
↓
state interfaces

write()

周期性：

复制代码

读取 command interfaces
↓
Safety Check
↓
构造PDO
↓
EtherCAT send

你之前的 Safety Layer 就非常适合在这里附近发挥作用。

Safety 应该在哪？

比如：

复制代码

Controller command
↓
Hardware Interface
↓
Validity Check
↓
Joint Limit
↓
Velocity Limit
↓
Command Timeout
↓
Fault State
↓
EtherCAT write

这样即使上层 Controller 出了异常：

复制代码

底层仍然有最后一道软件保护

这和你之前的目标设计完全一致。

Command Timeout 放哪里合适？

如果上层命令：

复制代码

长期没更新

底层不应该：

复制代码

永远重复最后一个command

可以维护：

复制代码

last_command_time

如果：

复制代码

now - last_command_time > timeout

则：

复制代码

进入safe state

这可以放在：

复制代码

controller

或者：

复制代码

hardware/safety layer

更严格场景通常底层还要自己兜底。

插件机制

Hardware Interface 通常会做成：

复制代码

plugin

比如：

复制代码

MyEthercatSystem

controller_manager 在运行时加载。

这意味着：

复制代码

controller_manager

不需要重新编译成只支持你的某一块硬件，它通过插件机制加载具体实现。

如果插件加载失败，怎么排？

比如：

复制代码

Failed to load hardware plugin

你可以从几层看：

复制代码

plugin XML配置
↓
.so有没有生成
↓
动态库路径
↓
依赖库
↓
符号

工具：

复制代码

ldd libmy_hardware.so

甚至：

复制代码

nm -D

这就是 C++ 工程能力和 ROS2 结合。

标准排查可以这样：

复制代码

Controller active?
↓
Command topic 有数据吗？
↓
Controller有没有收到？
↓
Command interface值有没有变化？
↓
Hardware write()有没有被调用？
↓
EtherCAT有没有发？
↓
Drive状态正确吗？

不要只停在：

复制代码

controller active