实时约束与高性能数据传递¶

难度：⭐⭐⭐⭐ | 建议用时：2 周 | 前置要求：线程管理与互斥同步线程与同步，原子操作与内存模型原子与内存模型，并行编程框架并行框架，C++语言核心/RAII与智能指针 RAII 与资源管理

前置自测¶

答不出两题以上，建议先复习线程调度、互斥锁、原子操作、容器分配和并行框架。实时系统不是“速度更快的普通程序”。它关心的是能否在规定时间内稳定完成，而不是平均耗时是否漂亮。

平均耗时 0.2ms 的函数，为什么仍可能不适合 1kHz 控制循环？
deadline、period、WCET、jitter 分别表示什么？
硬实时、软实时和准实时的区别是什么？
为什么实时循环里通常避免 new、delete、文件 I/O 和无界等待？
高优先级线程等待低优先级线程持有的 mutex，会产生什么问题？
priority inheritance 和 priority ceiling 分别试图解决什么？
SPSC 队列、MPMC 队列、双缓冲、不可变快照分别适合什么通信拓扑？
“零拷贝”一定更快吗？它会引入哪些生命周期约束？
ROS2 callback group 和 executor 为什么会影响实时路径？
如果一个数据通道追求“最新状态”，为什么不一定需要可靠传递每一帧？

本章目标¶

学完本章，你将能够：

用 deadline、period、WCET、jitter 和 tail latency 描述机器人软件的实时需求。
区分硬实时、软实时、准实时和高吞吐计算，并解释它们对架构的不同要求。
识别实时循环中的危险操作：动态内存分配、无界锁等待、阻塞 I/O、日志、异常路径和隐式容器扩容。
解释优先级反转、优先级继承、优先级天花板和锁粒度设计。
使用固定容量 SPSC ring buffer、预分配对象池、不可变快照和双阶段发布构造可预测数据通道。
判断什么时候用 mutex + deque 已经足够，什么时候才需要无锁或 wait-free 结构。
理解 cache locality、false sharing、NUMA 和内存带宽对实时抖动的影响。
解释 ROS2 executor、callback group、QoS、loaned message 与实时路径之间的工程边界。
建立最小可执行的测量体系：周期监控、deadline miss 统计、p99 延迟、分配计数和 trace。
完成 Mini SLAM Realtime Data Path：IMU 高频输入、LiDAR 帧队列、最新位姿发布、预分配缓冲和运行时监控。

知识树：

实时约束与高性能数据传递
├── 实时基础概念（20.1）
│   ├── deadline / period / WCET / jitter
│   ├── 硬实时 vs 软实时 vs 准实时
│   ├── RMA 调度理论
│   └── PREEMPT_RT 内核
├── 实时路径约束（20.2 - 20.3）
│   ├── 四类禁忌：分配、锁、I/O、日志
│   ├── 预分配与固定容量
│   └── 优先级反转与 PI/PC mutex
├── 高性能数据通道（20.4 - 20.7）
│   ├── SPSC 队列
│   ├── 最新状态发布
│   ├── 对象池与预分配块
│   └── 数据传递所有权模型
├── 硬件与平台（20.8 - 20.9）
│   ├── cache locality / false sharing
│   └── ROS2 executor / QoS 边界
└── 可观测性（20.10 - 20.12）
    ├── 热路径 / 冷路径分离
    ├── 实时安全日志
    └── 分位数监控与 trace

本章在课程中的位置：并行编程框架讨论的是”怎样让计算跑在多个核心上”。本章讨论的是“怎样让关键路径在规定时间内稳定完成”。两者有重叠，但目标不同。并行框架关注吞吐。实时系统关注最坏情况、长尾和资源边界。在机器人里，这个差异会直接决定系统是稳定运行，还是偶发卡顿后失控。

20.1 实时系统不是平均更快 ⭐⭐¶

这一节解决什么问题：实时系统的核心约束到底是什么？为什么"跑得快"和"跑得实时"是两回事？

工程问题：1kHz 控制循环意味着什么？¶

在深入实时系统的技术细节之前，先建立一个具体的物理直觉——1kHz 控制循环到底意味着什么？

一个四足机器人（如 Unitree Go2）在 trot 步态下行走时，每条腿的触地-离地周期约 300-500ms。在一个触地周期内，控制循环执行 300-500 次。每一次执行都要完成：读取 IMU 和关节编码器（约 0.05ms）、运行状态估计器（约 0.1ms）、计算 MPC 或 WBC 输出（约 0.2-0.5ms）、将力矩指令发送给电机驱动器（约 0.02ms）。整条路径的预算是 1ms——如果超过这个时间，力矩指令来不及发出，电机会在一个周期内失去控制输入。

1ms 有多短？ 光在 1ms 内传播约 300 公里，声音传播约 0.34 米。一个 60kg 的机器人在自由落体下 1ms 内下降约 0.005mm——看起来微不足道。但如果连续 10 次 deadline miss（10ms），下降就变成 0.5mm，关节角度偏差累积可能导致足端滑移或步态失调。如果 miss 持续 100ms（100 次），机器人可能已经开始倒向一侧，此时补救的控制力矩需要远大于正常值，电机可能饱和。这就是为什么实时系统关心**每一次**执行是否按时完成，而不是"平均下来够快就行"。

考虑一个 1kHz 控制循环。周期是 1ms。如果每个周期都要读取传感器、估计状态、计算控制量并写入执行器，那么这条路径必须在 1ms 内完成。

假设某个函数的耗时分布如下：

指标	耗时
平均	0.20ms
p90	0.25ms
p99	0.60ms
p99.9	1.40ms
最大观测值	3.20ms

平均耗时非常好。但 p99.9 已经超过 1ms。如果这个函数在控制循环中每周期调用，系统就会周期性错过 deadline。对于控制系统，偶发错过可能比稳定慢一点更危险。

反面失败：用吞吐 benchmark 证明实时安全¶

常见说法：

这个模块每秒能处理 5000 帧，所以 1kHz 没问题。

这句话把吞吐和实时性混在一起。每秒处理 5000 次，只说明平均每次 0.2ms。它没有说明最长一次会不会 5ms。也没有说明高优先级线程会不会被低优先级线程阻塞。更没有说明内存分配、页错误、日志 I/O、调度抢占会不会造成长尾。

实时安全需要回答的问题是：

在给定系统负载下，这条路径的最坏情况是否低于 deadline？
如果无法证明最坏情况，观测到的 p99.9/p99.99 是否有足够裕度？
miss 发生时系统如何降级？

抽象不变量：实时路径要有有界执行时间¶

实时路径的核心不变量是：

\[ C_{\text{path}} + B_{\text{block}} + J_{\text{sched}} < D \]

其中：

\(C_{\text{path}}\) 是计算本身耗时。
\(B_{\text{block}}\) 是阻塞等待时间。
\(J_{\text{sched}}\) 是调度抖动。
\(D\) 是 deadline。

普通高性能优化主要降低 \(C_{\text{path}}\) 的平均值。实时工程还要压低 \(B_{\text{block}}\) 和 \(J_{\text{sched}}\) 的上界。

如果某个操作可能无限等待，它就不适合放在硬实时路径：

等待 mutex。
等待磁盘写入。
等待网络包。
等待内存分配器内部锁。
等待另一个线程完成未知长度任务。

规则推导：deadline、period、WCET 与 jitter¶

周期任务可以用四个量描述：

概念	含义
Period \(T\)	任务触发间隔
Deadline \(D\)	任务必须完成的相对时间
WCET \(C\)	最坏情况执行时间
Jitter \(J\)	实际触发或完成时间相对理想周期的波动

对于 1kHz 控制任务：

\[ T = 1ms \]

如果要求每周期内完成：

\[ D = 1ms \]

如果测得或估计：

\[ C = 0.55ms,\quad J = 0.15ms \]

还剩：

\[ D - C - J = 0.30ms \]

这 0.30ms 是裕度。裕度不是浪费。它吸收缓存未命中、总线竞争、内核调度和偶发输入复杂度。

工程边界：硬实时、软实时与准实时¶

类型	错过 deadline 的后果	机器人例子
硬实时	系统级失败或安全风险	电机电流环、紧急制动
软实时	质量下降但可恢复	SLAM 可视化、地图发布
准实时	长尾需要受控	状态估计、局部规划
高吞吐	只关心总处理量	离线建图、批量数据转换

SLAM 前端常常是准实时。它需要低延迟和较小抖动，但偶尔丢一帧可以恢复。控制环更接近硬实时。日志、可视化和离线优化属于软实时或非实时。

不要把所有模块都按硬实时写。那会增加实现成本。也不要把控制路径当普通高吞吐任务写。那会带来不可预测风险。

实时性分类的理论根源：上面的四级分类不是工程经验的随意划分——它根植于实时系统理论中的**确定性保证层级**。理论上，任何计算任务都可以用它对时间约束的**承诺强度**来分类：

承诺强度	形式化表述	含义
硬实时	\(\forall\) 执行实例：\(R \leq D\)	所有情况下都必须满足
确定实时	\(P(R \leq D) = 1\)，但依赖概率模型	在给定概率模型下保证满足
软实时	\(E[\max(0, R - D)]\) 最小化	最小化超时的期望惩罚
尽力而为	无形式化承诺	尽量快，但不保证

硬实时需要的是**构造性证明**——通过分析所有可能的执行路径，证明最坏情况响应时间不超过 deadline。这和一般性能优化的目标完全不同：性能优化追求降低平均值，实时分析追求约束最坏值。

调度理论的基石：Rate Monotonic Analysis（RMA）。理解为什么机器人系统的任务要分优先级，需要了解调度理论中最基本的结论。Liu & Layland 在 1973 年证明了一个优美的定理：对于一组周期性任务，如果使用**静态优先级调度**（优先级在运行前确定，运行时不变），那么**Rate Monotonic**（RM）策略是最优的——RM 把更高的优先级分配给更短周期的任务。

具体来说，如果有 \(n\) 个周期任务，任务 \(i\) 的周期为 \(T_i\)，最坏执行时间为 \(C_i\)，那么 RM 调度可以保证所有 deadline 都被满足的**充分条件**是：

\[U = \sum_{i=1}^{n} \frac{C_i}{T_i} \leq n(2^{1/n} - 1)\]

其中 \(U\) 是**CPU 利用率**。当 \(n \to \infty\) 时，右边趋近于 \(\ln 2 \approx 0.693\)。这意味着：如果总利用率不超过约 69.3%，RM 调度保证所有任务都能按时完成。剩余约 30% 的 CPU 时间不是浪费——它是吸收最坏情况干扰所必需的裕度。

这个结果对机器人系统设计有直接指导意义。考虑一个典型的机器人控制系统：

任务	周期 \(T\)	WCET \(C\)	利用率 \(C/T\)
电流环	0.1ms	0.03ms	0.30
位置环	1ms	0.20ms	0.20
状态估计	5ms	1.00ms	0.20

总利用率 \(U = 0.70\)。\(n = 3\) 时的 RM 上界为 \(3(2^{1/3} - 1) \approx 0.780\)。因为 \(0.70 < 0.780\)，RM 调度可以保证所有任务按时完成。但这个上界是充分条件，不是必要条件——实际中可以用更精确的响应时间分析（RTA）验证更高利用率的可行性。

读到这里你可能会问："为什么不用 Earliest Deadline First（EDF）？" EDF 是动态优先级调度，理论上可以达到 100% 利用率上界——比 RM 的 69.3% 好得多。但 EDF 在过载时行为不可预测——如果某个任务偶尔超时，所有任务的 deadline 可能连锁失败。RM 在过载时更优雅——最低优先级的任务先失败，其他任务不受影响。这就是为什么工业实时系统（包括大多数机器人控制器）偏好 RM 而非 EDF。

实时系统的形式化定义与 PREEMPT_RT 内核¶

形式化定义：实时系统的本质不是"快"，而是"对时间行为有形式化保证"。严格地说，一个硬实时系统必须满足：对于所有可能的输入序列和所有可能的系统状态，关键任务的完成时间 \(R\) 不超过截止时间 \(D\)。这不是一个统计性质（不是"99.99% 的情况下满足"），而是一个确定性性质（"所有情况下满足"）。这就是为什么硬实时系统的设计约束如此严格——任何可能导致执行时间无界的操作（malloc、I/O 等待、优先级反转）都必须从关键路径中消除。

标准 Linux 内核为什么不是硬实时的？ 标准 Linux 内核被设计为"公平调度"系统——它追求所有任务的总吞吐量最大化，而不是某个特定任务的最坏响应时间最小化。具体来说，标准内核有三个阻碍硬实时的核心问题：

中断处理不可抢占：当硬件中断（如网卡收到数据包）发生时，CPU 会立刻跳转到中断处理函数。如果此时正在执行一个高优先级实时任务，这个任务会被中断处理打断，而且中断处理函数内部不能被其他任务抢占。如果中断处理函数执行时间较长（如处理大量网络数据包），实时任务就会被延迟数百微秒甚至毫秒。
内核临界区不可抢占：标准内核的很多路径持有自旋锁（spinlock），持锁期间关闭了抢占。如果一个低优先级任务进入了内核临界区（比如调用了 write() 系统调用），高优先级实时任务就无法抢占它，必须等待临界区结束。
定时器精度有限：标准内核的 hrtimer 精度虽然名义上是纳秒级，但实际唤醒延迟受调度器和中断上下文影响，可能产生数十到数百微秒的抖动。

PREEMPT_RT 补丁做了什么？ PREEMPT_RT（从 Linux 6.12 开始逐步合并进主线内核）从三个层面解决上述问题。理解每一层改造的关键不是记住技术细节，而是理解**它解决了上面三个问题中的哪一个，以及解决的物理机制是什么**。

第一层：中断线程化（Threaded Interrupts）——解决问题 1（中断不可抢占）。PREEMPT_RT 把大多数硬件中断处理函数转变为内核线程。中断发生时，内核只做最小的"应答"工作（通知硬件已收到中断），然后唤醒对应的中断线程来完成实际处理。这些中断线程和普通实时线程一样参与优先级调度——如果你的控制线程优先级高于网卡中断线程，控制线程就不会被网卡中断打断。这把"不可抢占的中断上下文"变成了"可调度的线程上下文"。量化效果：标准内核上，一次网卡中断处理可能持续 100-500 微秒，期间控制线程完全被阻塞；PREEMPT_RT 上，中断的"应答"部分只需几微秒，后续处理作为普通线程调度，控制线程可以抢占它。

第二层：优先级继承互斥锁（PI Futex / rt_mutex）——解决问题 2（内核临界区不可抢占）。PREEMPT_RT 把内核中大部分自旋锁替换为可休眠的互斥锁（rt_mutex），并内置优先级继承协议。当高优先级线程等待一个被低优先级线程持有的锁时，低优先级线程会被临时提升到与等待者相同的优先级——这确保了持锁者尽快完成并释放锁，避免优先级反转（下一节会详细讨论）。量化效果：标准内核上，一次 write() 系统调用持有自旋锁的时间可能达到数百微秒，期间高优先级任务无法抢占；PREEMPT_RT 上，锁被替换为可休眠锁，持锁者可以被抢占（抢占后通过 PI 确保持锁者尽快恢复运行），最坏阻塞时间从"整个临界区长度"降为"PI 响应延迟"（通常几微秒）。

第三层：高精度定时器与可抢占内核。PREEMPT_RT 确保几乎所有内核路径都可以被高优先级任务抢占（除了极少数真正需要原子性的短临界区）。配合高精度定时器，实时任务的唤醒抖动可以控制在几微秒以内。在典型的 PREEMPT_RT 系统上，一个 1kHz 周期任务的唤醒抖动从标准内核的 50-500 微秒降低到 5-30 微秒。

对机器人系统的实际意义：如果你的控制循环运行在标准 Linux 上，即使代码本身只需要 0.2ms，操作系统可能在任意时刻插入一次 2ms 的中断处理或内核临界区延迟——这就是为什么标准 Linux 上的控制循环 p99.9 往往难以控制。PREEMPT_RT 不能让你的算法更快，但它能让操作系统层面的不确定性变得可控——把"不知道什么时候会被打断"变成"只有比我优先级更高的任务才能打断我"。

读到这里你可能会问："那为什么不所有机器人都用 PREEMPT_RT？" 因为 PREEMPT_RT 有代价：把自旋锁替换为可休眠锁会增加上下文切换开销，整体吞吐量通常比标准内核低 5-15%。对于不需要硬实时的模块（SLAM 后端、可视化、日志），这个性能损失没有收益。所以实际的机器人系统架构往往是：控制处理器运行 PREEMPT_RT 或专用 RTOS，感知处理器运行标准 Linux——两者通过 EtherCAT、CAN 或共享内存通信。

代码验证：周期监控器¶

#include <algorithm>
#include <chrono>
#include <cstdint>
#include <iostream>
#include <vector>

class PeriodMonitor {
public:
    using Clock = std::chrono::steady_clock;

    explicit PeriodMonitor(std::chrono::microseconds deadline,
                           std::size_t max_samples)
        : deadline_(deadline),
          max_samples_(max_samples) {
        samples_.reserve(max_samples_);
    }

    void beginCycle() {
        start_ = Clock::now();
    }

    void endCycle() {
        const auto end = Clock::now();
        const auto elapsed =
            std::chrono::duration_cast<std::chrono::microseconds>(end - start_);
        if (samples_.size() < max_samples_) {
            samples_.push_back(elapsed.count());
        }
        if (elapsed > deadline_) {
            ++misses_;
        }
    }

    void printSummary() const {
        std::vector<std::int64_t> sorted = samples_;
        std::sort(sorted.begin(), sorted.end());
        if (sorted.empty()) {
            return;
        }

        auto pct = [&](double p) {
            const auto index = static_cast<std::size_t>(
                p * static_cast<double>(sorted.size() - 1));
            return sorted[index];
        };

        std::cout << "p50(us): " << pct(0.50) << "\n";
        std::cout << "p90(us): " << pct(0.90) << "\n";
        std::cout << "p99(us): " << pct(0.99) << "\n";
        std::cout << "misses: " << misses_ << "\n";
    }

private:
    std::chrono::microseconds deadline_;
    std::size_t max_samples_ = 0;
    Clock::time_point start_{};
    std::vector<std::int64_t> samples_;
    std::uint64_t misses_ = 0;
};

这个工具不证明硬实时。它建立最小观测能力。没有观测能力，实时讨论只会停留在感觉层面。注意它仍然是软实时监控工具：构造时预留容量后，endCycle() 在容量范围内不会重新分配；超过容量后只继续统计 deadline miss，不再记录全部样本。如果要把监控器放进硬实时控制循环，应使用固定容量 ring buffer 或二进制 trace 缓冲，并把排序、打印、分位数计算放到非实时线程。

本质洞察：实时系统不是"更快的普通系统"，而是**对最坏情况有承诺的系统**。普通高性能优化把平均耗时从 1ms 降到 0.5ms 是成功；实时系统把 p99.9 从 1.5ms 降到 0.9ms 才是成功。目标函数完全不同。

实时系统和普通高性能系统的关系，就像航空引擎和赛车引擎的关系：赛车引擎追求峰值功率，偶尔过热可以进站；航空引擎追求每一秒都在安全范围内运行，一次过热就可能导致灾难。机器人控制系统更接近航空引擎——不是要最快，而是要最稳。

如果实时系统只看平均耗时会怎样？假设一个 1kHz 控制循环平均耗时 0.2ms，但每 1000 次中有 1 次耗时 5ms。在 1 秒内就会出现 1 次 deadline miss。对于行走机器人，这意味着每秒有 1 次控制指令迟到——可能导致步态不协调甚至摔倒。吞吐 benchmark 看到的"5000 次/秒"完全掩盖了这个致命问题。

⚠️ 编程陷阱：在实时路径中调用 malloc 错误做法：控制循环内创建 std::vector 或 std::string，触发动态内存分配。现象：平均耗时正常，但 p99.9 偶尔出现数毫秒的尖峰。 根本原因：malloc 内部有锁（保护全局 free list）、可能触发 mmap 系统调用、可能触发页错误。这些操作的最坏情况耗时不可预测。 正确做法：在初始化阶段完成所有内存分配。运行阶段只读写已有对象，不创建新对象。

无锁数据结构为什么不等于实时安全？CAS 自旋的最坏情况分析¶

"无锁"（lock-free）这个词容易给人一种错觉——"没有锁就没有阻塞，没有阻塞就是实时安全的"。这个推理链条在每一步都有问题。

lock-free 的精确定义：lock-free 保证的是**系统级进展（system-wide progress）**——在任意时刻，至少有一个线程能在有限步内完成其操作。这意味着系统不会死锁或活锁。但它**不保证**某个特定线程能在有限步内完成——其他线程可能反复"插队"，让这个线程的 CAS 操作不断失败和重试。

CAS（Compare-And-Swap）重试的最坏情况：大多数 lock-free 数据结构的核心操作是 CAS 循环：

do {
    old_value = atomic_load(shared_variable);
    new_value = compute_next(old_value);
} while (!compare_and_swap(shared_variable, old_value, new_value));

每次 CAS 失败意味着另一个线程在上一次 load 和这次 CAS 之间修改了 shared_variable。线程必须重新读取新值、重新计算、重新尝试 CAS。在低竞争场景下，CAS 通常第一次就成功——开销约 10-50ns。但在高竞争场景下（N 个线程同时对同一个原子变量做 CAS），一个线程可能需要重试 N-1 次才能成功。

量化最坏情况：假设 8 个线程同时向一个 lock-free 栈做 push。每个线程的 CAS 操作大约需要 20ns（包含缓存一致性协议的开销）。如果线程 A 特别"倒霉"——每次它准备好 CAS 时，另一个线程刚好抢先成功：

第 1 次尝试：失败，另一个线程成功。耗时 20ns。
第 2 次尝试：失败，又一个线程成功。耗时 20ns。
...
第 7 次尝试：失败。耗时 20ns。
第 8 次尝试：成功。耗时 20ns。

最坏情况下，线程 A 的一次 push 操作耗时 \(8 \times 20\text{ns} = 160\text{ns}\)。如果线程数更多（比如 32 个），最坏情况变为 \(32 \times 20\text{ns} = 640\text{ns}\)。而且这还是理想化分析——实际中 CAS 失败后的缓存一致性开销（cache line bouncing）会随线程数超线性增长。

为什么 lock-free 不能等同于实时安全——形式化论证。将上面的分析严格化：设 lock-free 操作 \(O\) 在 \(N\) 个线程并发执行时，单个线程完成 \(O\) 需要的最坏 CAS 重试次数为 \(R_{\max}\)。对于典型的 lock-free 栈/队列，\(R_{\max}\) 与竞争线程数成线性关系：\(R_{\max} = O(N)\)。但 \(N\) 在运行时可能变化——如果系统动态创建线程（比如 ROS2 executor 为新的 callback group 创建线程），\(N\) 的上界可能不受应用层控制。

更严重的是 ABA 问题 引入的额外复杂度。ABA 问题发生在这样的场景中：线程 A 读到值 X，被挂起；线程 B 修改 X 为 Y 再改回 X；线程 A 恢复后做 CAS，看到值仍是 X，CAS 成功——但底层状态已经变了。解决 ABA 问题的标准方案是给指针附加版本号（tagged pointer），每次修改递增版本号。但版本号有有限位宽，理论上存在回绕的可能——虽然在实践中极不可能（64 位版本号回绕需要 \(2^{64}\) 次操作），但这破坏了形式化的有界性证明。

缓存一致性协议放大了竞争代价。上面的分析假设每次 CAS 耗时恒定（20ns），但实际上 CAS 失败后的重试涉及 MESI 协议的 cache line 状态转换（缓存优化与数据布局会详细讨论）。当多个核心同时对同一个 cache line 做 CAS 时，cache line 在核心之间反复传递（bouncing）：核心 A 把 cache line 改为 Modified 状态 → 核心 B 请求同一 cache line → 核心 A 必须把数据写回并降级为 Invalid → 核心 B 获得 Exclusive 状态 → 核心 C 又请求……每次传递的延迟约 40-100ns（取决于核心间互连拓扑），而且这个延迟随竞争线程数超线性增长——因为 interconnect 的带宽是有限的。在极端竞争下，CAS 的实际耗时可能比无竞争时慢 10-50 倍。

这就是"无锁 = 实时安全"这个等式失败的根本原因：lock-free 消除了**锁导致的线程阻塞**，但没有消除**缓存一致性协议导致的硬件级延迟**。在高竞争场景下，后者可能比前者更不可预测。

lock-free vs wait-free vs 实时安全：

保证级别	含义	最坏情况延迟	适合实时？
blocking（普通锁）	线程可能被无限期阻塞	无上界	不适合
lock-free	至少一个线程在有限步内完成	特定线程无上界	不一定
wait-free	每个线程在有限步内完成	有上界（取决于线程数）	条件适合
有界 wait-free	每个线程在固定步数内完成	固定上界	适合

真正的实时安全需要的是**有界最坏情况执行时间**。lock-free 不提供这个保证。wait-free 提供了，但 wait-free 数据结构通常更复杂、更慢（常数因子更大），而且很多常用操作（如动态大小队列的 push）很难做到真正的 wait-free。

实际的实时系统设计策略：与其追求 wait-free 数据结构，更实用的方法是**在架构层面消除竞争**。SPSC 队列（单生产者单消费者）就是一个例子——通过限制拓扑（只有一个读者和一个写者），CAS 重试次数的上界变为 1（因为只有一个竞争者）。这比在 MPMC 场景下追求 wait-free 更简单、更快、更可预测。

💡 概念误区：认为"无锁 = 实时安全" 新手想法："我用了 lock-free 队列，所以控制路径是实时安全的。" 实际上：lock-free 保证的是 progress（至少一个线程在推进），不保证 bounded latency。CAS 循环在高竞争下可能重试很多次。真正的实时安全需要 wait-free（每个操作有固定步数上限），或者设计上确保竞争极低。

🧠 思维陷阱：把所有路径都按硬实时标准设计 新手想法："安全起见，所有模块都不用 malloc、不用锁、不用异常。" 实际上：这会让整个系统的开发成本和复杂度飙升。正确做法是区分实时路径和非实时路径：控制环、状态估计是硬/准实时，用严格约束；地图维护、可视化、日志是软实时或非实时，可以使用普通 C++ 特性。 正确思维：先画出数据流图，标注哪些路径有 deadline，只对这些路径施加实时约束。

练习¶

[分析题] 一个函数的耗时分布为：平均 0.15ms，p99 0.40ms，p99.9 2.1ms，最大观测 8.3ms。如果这个函数在 1kHz 控制循环中每周期调用，估算每分钟发生多少次 deadline miss。
[设计题] 将一个 SLAM 系统的模块分为硬实时、准实时、软实时和非实时四类：IMU 预积分、SLAM 前端、后端优化、ROS2 参数回调、可视化发布、日志写入。解释分类理由。
[跨章综合题] 结合并行编程框架的 Amdahl 定律和本节的实时约束，解释为什么"把 SLAM 前端并行化到极致"可能对控制系统没有帮助——即使前端延迟降低，如果并行导致控制线程被抢占，整体系统反而更差。

20.2 实时路径的四类禁忌 ⭐⭐¶

工程问题：长尾通常来自隐藏操作¶

实时路径最怕的不是显眼的复杂算法。显眼算法至少容易测。真正危险的是看起来普通、内部却可能阻塞或分配的操作。

典型禁忌：

动态内存分配。
无界锁等待。
阻塞 I/O。
不可控日志和格式化。

这些操作在平均情况下可能很快。但它们的最坏情况难以约束。

反面失败：控制循环里 `push_back`¶

#include <vector>

void controlStep(const SensorSample& sample) {
    static std::vector<SensorSample> history;
    history.push_back(sample);
    computeControl(history);
}

push_back 大多数时候只是写一个元素。但当 capacity 不够时，它会重新分配、移动旧元素、释放旧内存。如果元素类型复杂，还可能触发构造、析构和异常路径。

在普通程序里这很正常。在 1kHz 控制循环里，这就是不可预测长尾。

抽象不变量：实时热路径不应改变资源集合¶

实时热路径的理想状态：

初始化阶段：
    分配所有内存
    打开所有文件或设备
    建立所有通信通道
    初始化所有容器容量

运行阶段：
    只读写已有对象
    不扩容
    不打开资源
    不等待无界操作

这不是说整个程序不能分配。而是实时热路径不能分配。后台线程可以维护地图、写日志、发布可视化。控制线程只应访问已经准备好的数据。

规则推导：容量上界来自系统需求¶

固定容量不是随便写一个大数。它应该来自输入上界：

IMU 频率: 400Hz
LiDAR 周期: 100ms
需要缓存: 2 个 LiDAR 周期
安全系数: 2
IMU 缓冲容量 >= 400 * 0.1 * 2 * 2 = 160

实际可以取 256，方便环形缓冲取模优化。容量过小会丢数据。容量过大浪费缓存并增加初始化成本。

工程边界：预分配不是无限容量¶

固定容量结构必须定义满了怎么办：

策略	含义	适用
拒绝写入	返回 false	关键数据不能覆盖
覆盖最旧	保留最新状态	姿态、速度、监控值
丢弃最新	保守保持已有数据	配置或命令
触发降级	通知上层减载	安全相关通道

不定义溢出策略，就是把系统行为交给偶然。

实时热路径的"初始化阶段分配、运行阶段不分配"策略，和航天软件中的"launch-before-launch"检查是同一个思想：在可以承受失败的时候（起飞前/初始化时）做所有可能出错的事情，在不能失败的时候（飞行中/运行时）只执行已经验证过的确定性操作。

如果不预分配固定容量、也不定义溢出策略会怎样？最常见的后果不是崩溃，而是"间歇性卡顿"——系统大部分时间正常，但每隔几分钟出现一次 20ms 的尖峰。这种 bug 极难复现和定位，因为它取决于 malloc 内部的碎片状态、操作系统的页面回收策略和当前内存压力，这些因素在不同时刻的组合几乎不可能重建。

⚠️ 编程陷阱：std::vector::push_back 在控制循环中的隐式扩容 错误做法：在每个控制周期中向一个 std::vector 追加传感器历史样本。现象：运行几百个周期后突然出现一次长耗时——从 0.1ms 跳到 3ms。 根本原因：push_back 在容量不足时会重新分配（通常 2 倍扩容），包括 malloc 新内存、拷贝旧元素、free 旧内存。这三个操作加起来可能远超 deadline。 正确做法：使用固定容量环形缓冲，或在初始化时 reserve 到已知上限。

💡 概念误区：认为 reserve() 能保证不再分配 新手想法："我调了 reserve(1000)，后面 push_back 就不会分配了。" 实际上：reserve 只能保证容量达到指定值。如果后续 push_back 超过 1000 个元素，仍然会触发扩容。而且 reserve 本身也要分配内存——如果在实时路径中首次调用，这次分配就落在了热路径上。 正确做法：在初始化阶段调用 reserve，运行阶段保证元素数量不超过已有容量，或使用固定容量容器。

练习¶

[估算题] IMU 频率 400Hz，LiDAR 周期 100ms，需要缓存 2 个 LiDAR 周期的 IMU 数据，安全系数 2。计算 IMU 环形缓冲的最小容量。解释为什么取 2 的幂次（如 256）比精确值更好。
[设计题] 为一个固定容量历史窗口定义四种溢出策略（拒绝、覆盖最旧、丢弃最新、触发降级），并说明每种策略分别适用于什么类型的数据通道。
[代码题] 修改 FixedWindow 模板，增加一个 overflowed() 方法返回是否发生过溢出，以及溢出次数统计。

代码验证：固定容量历史窗口¶

#include <array>
#include <cstddef>

template <class T, std::size_t Capacity>
class FixedWindow {
public:
    bool push(const T& value) {
        if (size_ < Capacity) {
            data_[(head_ + size_) % Capacity] = value;
            ++size_;
            return true;
        }

        data_[head_] = value;
        head_ = (head_ + 1) % Capacity;
        return false;
    }

    std::size_t size() const {
        return size_;
    }

    const T& at(std::size_t index) const {
        return data_[(head_ + index) % Capacity];
    }

private:
    std::array<T, Capacity> data_{};
    std::size_t head_ = 0;
    std::size_t size_ = 0;
};

这个结构不是线程安全容器。它只展示实时设计中的容量思想。跨线程使用时，还要放进合适的数据通道。

20.3 优先级反转：锁不是只有互斥成本 ⭐⭐⭐¶

工程问题：高优先级任务可能被低优先级任务间接阻塞¶

设有三个线程：

线程	优先级	任务
Control	高	1kHz 控制
Mapping	中	地图更新
Logging	低	记录数据

如果 Logging 持有一个 mutex。 Control 需要同一把 mutex，于是阻塞。此时 Mapping 抢占 Logging。 Control 虽然优先级最高，却要等 Mapping 和 Logging。这就是优先级反转。

反面失败：实时线程读取后台配置时直接锁共享对象¶

std::mutex config_mutex;
Config config;

Command controlStep(const State& state) {
    std::lock_guard<std::mutex> lock(config_mutex);
    return computeCommand(state, config);
}

void updateConfig(Config new_config) {
    std::lock_guard<std::mutex> lock(config_mutex);
    config = std::move(new_config);
}

如果 updateConfig 在低优先级线程中持锁较久，高优先级控制线程会被阻塞。即使持锁平均很短，也仍然有长尾风险。

抽象不变量：实时线程不能等待不可控持锁时间¶

实时线程访问共享数据有几种更稳妥方式：

初始化后只读，不再修改。
不可变快照，用原子指针发布。
双缓冲或三缓冲，并有明确生命周期协议。
非实时线程写入，实时线程在周期边界尝试获取，不成功则使用旧值。
用支持优先级继承的 mutex，并严格限制临界区长度。

没有一种方案永远最好。关键是让等待时间有界。

规则推导：阻塞时间进入响应时间分析¶

一个简化响应时间模型：

\[ R_i = C_i + B_i + \sum_{j \in hp(i)}\left\lceil\frac{R_i}{T_j}\right\rceil C_j \]

其中：

\(R_i\) 是任务 \(i\) 的响应时间。
\(C_i\) 是任务自身计算时间。
\(B_i\) 是被低优先级任务阻塞的最长时间。
\(hp(i)\) 是优先级高于任务 \(i\) 的任务集合。
\(T_j\) 是高优先级任务周期。

如果 \(B_i\) 没有上界，\(R_i\) 就没有可用上界。这就是实时路径不喜欢普通锁的根本原因。

工程边界：优先级继承不是免死金牌¶

Linux pthread mutex 可以配置 priority inheritance。它可以缓解优先级反转。但它不解决：

临界区太长。
锁嵌套导致复杂等待。
持锁期间执行 I/O。
持锁期间内存分配。
高优先级线程频繁竞争同一锁。

优先级继承是安全网，不是设计目标。实时设计仍应减少高优先级线程的锁依赖。

Mars Pathfinder 事件：优先级反转的经典工程案例¶

优先级反转不是教科书上的抽象问题——1997 年 NASA 的 Mars Pathfinder 火星探测器就因为这个问题差点任务失败，这个事件成为实时系统设计中被引用最多的反面教材。理解这个事件不仅仅是"听一个故事"，而是要从中推导出"为什么 mutex 会导致低优先级阻塞高优先级"的形式化机制。

先推导机制，再看事件。优先级反转的产生需要三个条件同时满足：

共享资源的互斥约束：两个不同优先级的任务需要访问同一个受 mutex 保护的共享数据。mutex 的语义保证一次只有一个持有者——这是数据一致性的基本要求。
抢占式调度：操作系统按优先级决定哪个任务获得 CPU。高优先级任务可以抢占低优先级任务——这是实时性的基本要求。
优先级不连续：存在中间优先级任务，它不需要该 mutex，但可以抢占持锁的低优先级任务——这是问题的放大器。

当这三个条件同时满足时，会产生一个**逻辑矛盾**：mutex 语义要求"持锁者先完成临界区"，但调度器不知道低优先级任务持有一把高优先级任务需要的锁。调度器只根据优先级做抢占决策，所以中间优先级任务合法地抢占了持锁者——结果是高优先级任务的等待时间不再取决于临界区长度（有界），而取决于所有中间优先级任务的执行时间之和（可能无界）。

用形式化语言表述：设低优先级任务 \(L\) 在时刻 \(t_0\) 获取 mutex \(M\)，临界区长度为 \(C_L\)。如果没有中间优先级任务干扰，高优先级任务 \(H\) 在 \(t_0 + C_L\) 之前就能获取 \(M\)——阻塞时间有界为 \(C_L\)。但如果存在中间优先级任务集合 \(\{M_1, M_2, \ldots, M_k\}\)，它们可以在 \(L\) 持锁期间抢占 \(L\)，那么 \(H\) 的阻塞时间变为：

\[B_H = C_L + \sum_{j=1}^{k} C_{M_j}\]

其中 \(C_{M_j}\) 是中间优先级任务 \(M_j\) 的执行时间。如果中间优先级任务数量多或执行时间长，\(B_H\) 可以任意大——这就是优先级反转导致"高优先级任务被无限期阻塞"的精确原因。

事件背景：Pathfinder 使用 VxWorks 实时操作系统。系统中有三个关键任务：

bc_sched（高优先级，\(H\)）：管理数据总线的调度任务，负责在各个设备之间分发通信时间片。如果这个任务不能按时执行，总线通信就会中断。
通信任务（中优先级，\(M_1\)）：负责处理地球-火星之间的通信数据包，计算量较大。
气象任务（低优先级，\(L\)）：收集温度、风速等气象数据并写入共享的"信息总线"数据结构。

问题的触发：bc_sched 和气象任务共享一个互斥信号量来保护"信息总线"数据结构。正常情况下，气象任务持锁时间很短（只是写入几个数值，\(C_L\) 约几十微秒），bc_sched 几乎不会等待。但以下序列在火星表面真实发生了——它精确对应上面推导的三个条件：

低优先级气象任务 \(L\) 获取互斥信号量 \(M\)，开始写入数据。（条件 1 满足：\(L\) 持有 \(M\)）
中优先级通信任务 \(M_1\) 被唤醒（收到地球发来的数据包），抢占了气象任务。通信任务计算量大，可能持续运行数百毫秒。（条件 2+3 满足：\(M_1\) 不需要 \(M\)，但合法抢占了持锁的 \(L\)）
高优先级 bc_sched（\(H\)）到达执行时间，尝试获取互斥信号量——但信号量被气象任务持有，bc_sched 阻塞。（\(H\) 的阻塞时间 \(B_H = C_L + C_{M_1}\)，其中 \(C_{M_1}\) 可达数百毫秒）
此时系统中：bc_sched 在等气象任务释放锁，气象任务在等通信任务释放 CPU，通信任务在忙碌计算。高优先级任务被中优先级任务间接阻塞——经典的优先级反转。
bc_sched 长时间无法执行，总线调度超时，看门狗定时器触发系统复位。探测器反复重启。

NASA 的修复方案：VxWorks 本身支持互斥信号量的优先级继承选项（SEM_INVERSION_SAFE），但开发团队在创建信号量时没有启用这个选项。JPL 工程师通过远程上传修改了信号量创建参数，启用了优先级继承。修复后：当 bc_sched 等待气象任务持有的信号量时，气象任务的优先级被临时提升到与 bc_sched 相同。这样通信任务（中优先级）就无法抢占气象任务（现在临时是高优先级），气象任务尽快完成临界区操作并释放信号量，bc_sched 得以按时执行。

优先级继承 vs 优先级天花板：Mars Pathfinder 用的是优先级继承（priority inheritance），但实时系统中还有另一种解决方案——优先级天花板（priority ceiling）。两者的区别很重要：

特性	优先级继承 (PI)	优先级天花板 (PC)
何时提升优先级	高优先级线程实际等待时才提升持锁者	线程获取锁的瞬间就提升到天花板值
天花板值	动态等于最高等待者的优先级	静态配置为所有可能使用该锁的任务中的最高优先级
阻塞次数上限	可能被多个锁分别阻塞	每个任务最多被阻塞一次
实现复杂度	需要跟踪等待链	更简单，不需要等待链
死锁预防	不能自动预防	可以预防（如果配置正确）
代价	提升可能延迟（要等到竞争发生）	可能不必要地提升优先级（如果没有竞争）

优先级天花板的优势在于它的确定性更强——任务获取锁的瞬间就提升到最高可能优先级，确保持锁期间不会被任何可能需要该锁的任务抢占。这意味着持锁期间根本不可能发生优先级反转。代价是即使没有竞争，持锁者也会以高优先级运行，可能延迟其他中间优先级任务。

对机器人系统设计的教训：Mars Pathfinder 事件的教训不仅仅是"记得开 PI"。更深层的教训是：实时系统中的共享资源设计必须显式考虑优先级关系。具体来说：

如果高优先级任务和低优先级任务必须共享数据，首选方案是完全避免共享——用不可变快照、双缓冲或消息传递代替共享锁。
如果不可避免地需要锁，必须评估最坏情况阻塞时间，并将其纳入响应时间分析。
如果使用锁，必须启用优先级继承或优先级天花板——不启用等于在系统中埋了一个定时炸弹。
即使启用了 PI/PC，临界区也必须尽可能短——PI 只是把阻塞时间从"无界"变成"等于临界区长度"，如果临界区本身很长（比如持锁时做 I/O 或分配内存），PI 也救不了你。

本质洞察：优先级反转的本质不是"锁的问题"，而是**调度器不知道低优先级任务的完成影响着高优先级任务的进展**。优先级继承通过临时提升持锁者的优先级来告知调度器这个依赖关系——但它只是一个修补措施，真正的解决方案是减少高低优先级线程之间的共享资源。

优先级反转就像医院急诊室：危重病人（高优先级）需要一台设备，但设备正被轻症病人（低优先级）使用。如果中间来了普通病人（中优先级）并抢占了轻症病人的时间，危重病人就被无限期延误。优先级继承相当于临时把轻症病人也标记为"加急"，让他尽快完成并释放设备。

如果不使用优先级继承 mutex，而是让实时线程用 try_lock 会怎样？try_lock 不阻塞——如果锁被占用就立刻返回失败。这避免了优先级反转，但代价是实时线程可能拿不到最新配置。对于"最新状态"类的数据，这通常可以接受——用旧值继续运行比等待新值更安全。对于"必须获取"的资源（如硬件寄存器），try_lock 不适用。

⚠️ 编程陷阱：实时线程中使用 std::mutex 而非 PI mutex 错误做法：std::lock_guard<std::mutex> lock(config_mutex); 在 1kHz 控制线程中。现象：偶发控制周期从 0.2ms 跳到 10ms+，且与后台配置更新时间高度相关。 根本原因：std::mutex 不保证优先级继承。低优先级配置更新线程持锁时，中优先级线程可能抢占它，导致高优先级控制线程被间接阻塞。 正确做法：使用 pthread priority inheritance mutex，或更好地，用不可变快照（原子操作与内存模型）完全避免实时线程加锁。

🧠 思维陷阱：认为优先级继承能解决所有优先级反转 新手想法："我配置了 PI mutex，优先级反转问题就完全解决了。" 实际上：PI 只处理直接锁依赖。如果存在锁链（A 等 B 的锁，B 等 C 的锁），或者持锁期间做了 I/O、分配、日志等不可控操作，PI 也无法帮你。PI 继承的是优先级，不继承"尽快完成"的能力。 正确思维：PI 是防护网，设计时仍应最小化实时线程的锁依赖，理想情况下实时线程完全不加锁。

练习¶

[场景分析题] 三个线程：Control（高优先级，1kHz）、Mapping（中优先级，10Hz）、Logger（低优先级，1Hz）。Control 和 Logger 共享一把 mutex 保护配置对象。画出优先级反转发生的时序图，说明 Control 最多可能被延迟多久。
[设计题] 重新设计上述场景，使 Control 线程完全不需要加锁即可读取最新配置。提示：考虑 std::atomic<std::shared_ptr<const Config>>。
[代码题] 封装一个 TryLockGuard，如果 try_lock 失败返回一个标志，让调用者使用旧值继续运行。

代码验证：pthread 优先级继承 mutex 的封装¶

#include <pthread.h>
#include <stdexcept>

class PriorityInheritanceMutex {
public:
    PriorityInheritanceMutex() {
        pthread_mutexattr_t attr;
        if (pthread_mutexattr_init(&attr) != 0) {
            throw std::runtime_error("pthread_mutexattr_init failed");
        }
        if (pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT) != 0) {
            pthread_mutexattr_destroy(&attr);
            throw std::runtime_error("pthread_mutexattr_setprotocol failed");
        }
        if (pthread_mutex_init(&mutex_, &attr) != 0) {
            pthread_mutexattr_destroy(&attr);
            throw std::runtime_error("pthread_mutex_init failed");
        }
        pthread_mutexattr_destroy(&attr);
    }

    ~PriorityInheritanceMutex() {
        pthread_mutex_destroy(&mutex_);
    }

    PriorityInheritanceMutex(const PriorityInheritanceMutex&) = delete;
    PriorityInheritanceMutex& operator=(const PriorityInheritanceMutex&) = delete;

    void lock() {
        pthread_mutex_lock(&mutex_);
    }

    void unlock() {
        pthread_mutex_unlock(&mutex_);
    }

private:
    pthread_mutex_t mutex_{};
};

这段代码用于说明机制。真实项目还需要处理平台差异、初始化失败策略、调度权限和测试。

20.4 SPSC 队列：最常见的高频数据通道 ⭐⭐¶

工程问题：很多传感器通道天然是单生产者单消费者¶

典型拓扑：

IMU callback  ->  IMU buffer  ->  Frontend integration
Encoder read  ->  State buffer ->  Control loop
LiDAR driver  ->  Frame queue  ->  Scan processing

如果只有一个生产者和一个消费者，SPSC 队列可以比通用 MPMC 队列更简单、更快、更可预测。拓扑越明确，数据结构越简单。

原理与实现见 18.9，本节只补充实时视角¶

SPSC ring buffer 的完整原理（为什么单写者 tail / 单写者 head 就能去掉 CAS、为什么要留一个空槽区分满和空、release/acquire 如何建立发布关系）以及可运行的模板实现，已经在「原子操作与内存模型」的 18.9 SPSC Ring Buffer 中详细展开，这里不再重复。一句话回顾：

生产者独占写 tail，消费者独占写 head；一个槽位不会同时被读写。
empty: head == tail        full: next(tail) == head（留一个空槽避免二义）

本节站在实时约束的角度，只补充一个 18.9 没有强调的工程要点——满队列时的丢弃策略必须按数据通道显式定义。

工程边界：满队列策略必须显式（实时视角）¶

SPSC 队列满时，生产者的入队操作（18.9 中的 push/tryPush）会返回 false。这个 false 不能被忽略——必须按数据通道的语义显式决定怎么处理：

通道	满时策略
高频 IMU	记录溢出，触发降级或增大容量
最新状态	覆盖旧值
控制命令	拒绝并报告
可视化消息	丢弃最新或旧消息都可接受
安全事件	不应使用易丢队列

实时系统不怕丢弃。它怕没有定义地丢弃。

SPSC 队列和快递柜是同一个心智模型：快递员（生产者）放包裹，收件人（消费者）取包裹。柜子满了快递员就不能放入。一般快递柜不需要多个快递员同时操作同一个格子——所以设计比多人共用的储物柜（MPMC）简单得多。

⚠️ 多生产者误用的危险见 18.9：把 SPSC 用在多个生产者写同一队列的场景，是最隐蔽的无锁 bug 之一（低负载正常、高并发偶发数据损坏）。完整的反例分析和"该改用 MPSC / 每生产者一个 SPSC"的处理方式见 18.9。本节只强调：实时路径上一旦拓扑可能变成多写者，就不能再用 SPSC。

💡 概念误区：认为 MPMC 队列比 SPSC 队列"更通用所以更好" 新手想法："用 MPMC 总没错，它能处理任何情况。" 实际上：MPMC 的通用性来自更多原子操作、更复杂的内存回收和更难分析的竞争路径。对于明确的单生产者单消费者拓扑，SPSC 的性能和可预测性都更好。选择数据结构应匹配拓扑，而不是追求"最通用"。

练习¶

[设计题] 针对上表的五类通道（高频 IMU、最新状态、控制命令、可视化消息、安全事件），分别说明 SPSC 队列满时应采取的丢弃/降级策略，并解释为什么"没有定义的丢弃"在实时系统里比"明确丢弃"更危险。（环形缓冲本身的实现与边界测试见 18.9。）
[设计题] 一个系统有 3 个 IMU（主 IMU + 2 个备份 IMU），每个 IMU 有独立回调，数据需要汇入前端处理线程。这个拓扑应该用几个 SPSC 队列还是一个 MPSC 队列？分析各方案的权衡。

20.5 最新状态发布：队列不是唯一答案 ⭐⭐¶

工程问题：有些消费者只需要最新值¶

位姿显示、状态监控、控制器读取估计状态，往往只需要最新值。如果用队列保存每一次发布，消费者落后时会处理过期数据。

例如控制器需要当前估计位姿。处理 20ms 前的位姿没有意义。这时“最新状态发布”比“可靠队列”更合适。

反面失败：控制线程补处理旧状态¶

Estimator 400Hz 发布状态
Controller 1kHz 读取队列
某次调度抖动后队列积压 20 条
Controller 逐条 pop 并处理旧状态

控制线程忙于追赶历史。它越努力，越偏离“使用当前状态”这个目标。

抽象不变量：最新状态通道保存的是快照，不是历史¶

最新状态通道的不变量：

读者要么读到旧完整快照。
要么读到新完整快照。
不能读到半写快照。
写者不能修改读者正在使用的对象。

原子操作与内存模型讨论过：简单“双缓冲 + active index”在 C++ 里容易忽略读者生命周期。教学和非硬实时路径中，不可变快照更容易写对。硬实时路径中，应使用预分配缓冲和明确占用协议。

代码验证：不可变状态快照¶

#include <atomic>
#include <memory>

struct RobotState {
    double x = 0.0;
    double y = 0.0;
    double yaw = 0.0;
    double vx = 0.0;
    double vy = 0.0;
    double wz = 0.0;
    std::uint64_t stamp_ns = 0;
};

class LatestState {
public:
    void publish(const RobotState& state) {
        auto snapshot = std::make_shared<const RobotState>(state);
        current_.store(snapshot, std::memory_order_release);
    }

    RobotState load() const {
        auto snapshot = current_.load(std::memory_order_acquire);
        if (!snapshot) {
            return {};
        }
        return *snapshot;
    }

private:
    std::atomic<std::shared_ptr<const RobotState>> current_{};
};

这个实现语义清楚。但 make_shared 会分配内存。因此它适合中低频状态发布、配置发布和非硬实时路径。如果用于硬实时控制循环，需要改成预分配快照池。

工程边界：实时版本需要生命周期协议¶

预分配快照池可以避免分配。但它要解决“读者什么时候释放槽位”的问题。常见方案：

单读者：读者复制到本地后立即释放。
多读者：引用计数或读者位图。
最新值：允许覆盖，但读者只能读 atomic 字段或有版本校验。
硬实时：把复制成本固定在小对象内，避免复杂共享生命周期。

很多系统最终会选择：控制路径只复制一个小状态结构。大点云、大图像、大地图块走非实时通道。

⚠️ 编程陷阱：std::make_shared 在最新状态发布的热路径中分配内存 错误做法：每次发布状态都调用 std::make_shared<const RobotState>(state)。现象：在高频发布（>100Hz）场景下，p99 延迟偶尔出现尖峰。 根本原因：make_shared 每次调用都走默认分配器，可能触发 malloc 内部锁或系统调用。 正确做法：对于硬实时路径，使用预分配的快照池或固定大小双缓冲。shared_ptr 方案只适合中低频发布。

练习¶

[设计题] 比较三种最新状态发布方案：atomic<shared_ptr<const T>>、双缓冲 + 原子索引、SeqLock。从内存分配、延迟、实现复杂度三个维度做表格对比。
[代码题] 实现一个预分配快照池版本的 LatestState，使发布操作完全不触发 new/malloc。

20.6 对象池与预分配数据块 ⭐⭐¶

工程问题：大消息传递不适合频繁拷贝和分配¶

一帧点云可能包含几十万点。如果每个阶段都复制一次：

driver -> queue -> frontend -> filter -> matcher -> publisher

内存带宽会被快速消耗。更糟糕的是，每次创建新 std::vector<Point> 都可能分配大块内存。实时路径中，大块分配会造成明显长尾。

反面失败：每帧创建临时点云容器¶

PointCloud preprocess(const PointCloud& input) {
    PointCloud output;
    for (const auto& p : input.points) {
        if (isValid(p)) {
            output.points.push_back(transform(p));
        }
    }
    return output;
}

这段代码在普通 C++ 中很自然。但它可能多次扩容。如果每帧调用，分配器行为会进入实时路径。

抽象不变量：数据块所有权显式流动¶

对象池的核心是：

初始化时创建固定数量数据块
生产者从 free list 取一个空块
生产者填充数据
生产者把块交给消费者
消费者处理后归还 free list

数据块在系统中流动。内存不在热路径中创建和销毁。

代码验证：固定容量点云块¶

#include <array>
#include <cstddef>

template <class Point, std::size_t MaxPoints>
class FixedPointBlock {
public:
    bool push(const Point& point) {
        if (size_ >= MaxPoints) {
            return false;
        }
        points_[size_] = point;
        ++size_;
        return true;
    }

    void clear() {
        size_ = 0;
    }

    std::size_t size() const {
        return size_;
    }

    const Point& operator[](std::size_t index) const {
        return points_[index];
    }

    Point& operator[](std::size_t index) {
        return points_[index];
    }

private:
    std::array<Point, MaxPoints> points_{};
    std::size_t size_ = 0;
};

这个结构牺牲了动态容量，换来的是容量上界和无分配热路径。这里体现了实时系统设计的一个核心权衡：用灵活性换确定性。普通程序追求"能处理任意大小的输入"，实时程序追求"无论输入是什么，处理时间都有上界"。固定容量点云块就是这个权衡的具体体现——它不如 std::vector 灵活，但它保证了运行阶段不触发任何内存分配。

对象池句柄：用 RAII 表达归还¶

对象池如果要求手动归还，很容易泄漏块。 RAII 句柄可以把归还绑定到生命周期。

#include <array>
#include <bitset>
#include <cstddef>
#include <optional>

template <class Block, std::size_t Count>
class FixedBlockPool {
public:
    class Handle {
    public:
        Handle() = default;

        Handle(FixedBlockPool* pool, std::size_t index)
            : pool_(pool), index_(index) {}

        ~Handle() {
            reset();
        }

        Handle(const Handle&) = delete;
        Handle& operator=(const Handle&) = delete;

        Handle(Handle&& other) noexcept {
            pool_ = other.pool_;
            index_ = other.index_;
            other.pool_ = nullptr;
        }

        Handle& operator=(Handle&& other) noexcept {
            if (this != &other) {
                reset();
                pool_ = other.pool_;
                index_ = other.index_;
                other.pool_ = nullptr;
            }
            return *this;
        }

        Block& get() {
            return pool_->blocks_[index_];
        }

        const Block& get() const {
            return pool_->blocks_[index_];
        }

        explicit operator bool() const {
            return pool_ != nullptr;
        }

    private:
        void reset() {
            if (pool_) {
                pool_->release(index_);
                pool_ = nullptr;
            }
        }

        FixedBlockPool* pool_ = nullptr;
        std::size_t index_ = 0;
    };

    std::optional<Handle> tryAcquire() {
        for (std::size_t i = 0; i < Count; ++i) {
            if (!used_.test(i)) {
                used_.set(i);
                blocks_[i].clear();
                return Handle(this, i);
            }
        }
        return std::nullopt;
    }

private:
    void release(std::size_t index) {
        used_.reset(index);
    }

    std::array<Block, Count> blocks_{};
    std::bitset<Count> used_{};
};

这个示例不是线程安全对象池。它展示所有权模型。跨线程对象池需要把 free list 做成受控队列或用锁保护非实时路径。

工程边界：对象池也可能造成优先级问题¶

对象池满了怎么办？

策略	结果
阻塞等待空块	实时路径可能卡死
返回失败	上层必须降级
覆盖旧块	需要明确谁会受影响
动态扩容	破坏实时边界

实时路径通常选择返回失败或丢弃非关键数据。不要在池满时偷偷扩容。那会让对象池失去意义。

⚠️ 编程陷阱：对象池满时 fallback 到 new 错误做法：if (pool_empty()) return new PointCloud();——池满时偷偷走系统分配器。现象：长期运行后偶发长尾延迟，且与池满时刻高度相关。 根本原因：fallback 到 new 破坏了对象池"所有分配在初始化阶段完成"的承诺。实时路径重新引入了 malloc 的不可预测性。 正确做法：返回失败标志，让上层决定是降级、丢弃还是扩展池容量（在非实时路径中）。

练习¶

[设计题] 一个对象池管理 PointCloud 块，容量为 8。如果 LiDAR 10Hz、每帧占用 1 个块、前端处理需要 3 帧、后端偶尔回溯 2 帧，池容量是否足够？如果不够，应该增加多少？
[代码题] 为 FixedBlockPool 的 Handle 增加移动语义测试：验证移动后源 Handle 不再持有资源，且移动目标正确归还。

20.7 高性能数据传递：拷贝、移动、共享与借用 ⭐⭐¶

工程问题：数据传递成本来自内存流量和生命周期¶

C++ 提供多种传递方式：

方式	成本	生命周期风险
拷贝值	内存带宽高	简单
移动值	避免深拷贝	源对象被移走
`shared_ptr<const T>`	避免数据拷贝	引用计数成本
原始指针/引用	成本低	生命周期容易错
借用消息	可零拷贝	受中间件约束
预分配块索引	成本低且可控	池管理复杂

零拷贝不是唯一目标。生命周期清楚同样重要。

反面失败：为了零拷贝返回悬空引用¶

const PointCloud& makeFilteredCloud(const PointCloud& input) {
    PointCloud filtered;
    filter(input, filtered);
    return filtered;
}

这当然是悬空引用。更隐蔽的是返回内部可复用缓冲的引用：

const PointCloud& getLatestCloud();

如果生产者下一帧复用同一缓冲，消费者持有的引用就变成了正在被改写的数据。零拷贝如果没有生命周期协议，比拷贝更危险。

抽象不变量：传递方式必须说明所有权¶

每条数据通道都应回答：

谁创建数据？
谁可以修改数据？
谁负责释放或归还？
消费者可以持有多久？
满载时丢弃还是阻塞？

如果这些问题无法回答，不要急着谈零拷贝。

规则推导：拷贝并不总是坏事¶

小对象拷贝往往比共享生命周期便宜。例如：

struct CompactState {
    double q[7];
    double dq[7];
    std::uint64_t stamp_ns;
};

这个对象只有百余字节。控制循环每周期复制一次通常可接受。相比之下，引入 shared_ptr、引用计数、跨线程生命周期和缓存抖动，可能更复杂。

大点云、大图像、大地图块才更需要借用或预分配块。

数据传递中的所有权问题和现实世界的图书馆借阅是同构的：拷贝相当于复印一本书（安全但浪费纸张），移动相当于把书从一个人手里交给另一个人（高效但源方不再有书），shared_ptr 相当于图书馆借阅系统（多人可以同时借阅，最后一人还书时回收），裸指针相当于口头约定"我的书放在桌上你自己拿"（高效但没有制度保障）。

⚠️ 编程陷阱：零拷贝返回内部缓冲引用导致悬空 错误做法：const PointCloud& getLatestCloud() 返回内部复用缓冲的引用。现象：消费者使用引用时，生产者已经用下一帧数据覆盖了同一缓冲。结果读到半新半旧的数据。 根本原因：零拷贝通过共享内存避免了复制成本，但引入了生命周期耦合。如果没有明确的占用/释放协议，引用随时可能失效。 正确做法：小对象直接拷贝；大对象用预分配块+RAII 句柄+明确归还协议。

练习¶

[分类题] 对以下数据通道选择合适的传递方式（拷贝/移动/shared_ptr/预分配块索引）：(a) 6-DOF 位姿（约 50 字节）；(b) 一帧点云（100K 点，约 1.6MB）；(c) 配置更新（约 200 字节，低频）；(d) 可视化图像（640x480，约 300KB）。
[分析题] shared_ptr<const T> 的引用计数更新是原子操作。如果 100 个线程同时持有同一个 shared_ptr，原子引用计数的缓存争用是否会成为问题？在什么规模下会开始影响性能？

代码验证：移动传递所有权¶

#include <utility>
#include <vector>

struct PointCloud {
    std::vector<PointXYZI> points;
    std::uint64_t stamp_ns = 0;
};

class CloudSink {
public:
    void submit(PointCloud cloud) {
        latest_ = std::move(cloud);
    }

private:
    PointCloud latest_;
};

按值传参再移动是一种清楚的所有权转移接口。调用者可以传临时对象，也可以显式 std::move。但它仍然可能在构造 PointCloud 时分配。实时路径中应结合预分配策略使用。

工程问题：实时抖动不只来自锁¶

即使没有锁、没有分配，内存访问也可能造成长尾。原因包括：

cache miss。
false sharing。
内存带宽饱和。
NUMA 远端访问。
其他核心写同一 cache line。

缓存优化与数据布局会深入讲数据布局。本章先把这些问题放进实时上下文。

反面失败：每个线程写相邻计数器¶

struct Counter {
    std::uint64_t value = 0;
};

std::array<Counter, 8> counters;

void count(int thread_id) {
    ++counters[thread_id].value;
}

不同线程写不同元素，看起来没有共享变量。但多个 Counter 可能位于同一个 cache line。一个线程写自己的计数器，会让其他核心的同一 cache line 失效。这就是 false sharing。

抽象不变量：并发写入要按 cache line 隔离¶

高频写入的线程本地指标可以 padding：

#include <new>

struct alignas(std::hardware_destructive_interference_size) PaddedCounter {
    std::uint64_t value = 0;
};

这不是为了“美观”。而是避免多个核心反复抢同一 cache line 的所有权。

工程边界：padding 会增加内存占用¶

不是所有结构都需要 padding。只对高频并发写入、跨线程共享 cache line 的数据使用。对普通对象滥用 padding 会浪费缓存，反而降低性能。

代码验证：实时指标的 padding 布局¶

#include <atomic>
#include <cstdint>
#include <new>

struct alignas(std::hardware_destructive_interference_size) RealtimeCounter {
    std::atomic<std::uint64_t> value{0};

    void increment() {
        value.fetch_add(1, std::memory_order_relaxed);
    }
};

struct RuntimeMetrics {
    RealtimeCounter control_cycles;
    RealtimeCounter deadline_misses;
    RealtimeCounter dropped_imu;
    RealtimeCounter dropped_clouds;
};

relaxed 足够用于统计计数。它不发布其他数据。如果某个计数器同时承担状态同步作用，就不能随便使用 relaxed。

⚠️ 编程陷阱：false sharing 导致实时统计计数器抖动 错误做法：std::array<std::atomic<uint64_t>, 8> counters; 中多个线程写相邻计数器。现象：计数器更新耗时不稳定，偶尔出现微秒级尖峰。 根本原因：多个原子计数器位于同一 cache line，一个核心的写操作会让其他核心的同一 cache line 失效，即使它们写的是不同地址。 正确做法：对高频并发写的计数器使用 alignas(std::hardware_destructive_interference_size) padding。

练习¶

[实验题] 构造一个 false sharing 示例：4 个线程分别递增 counters[0..3]，分别测量有 padding 和无 padding 的每秒递增次数。预期差异在什么量级？
[分析题] std::hardware_destructive_interference_size 在不同平台上可能是多少？如果平台不提供这个常量，应该用什么值作为保守默认？

20.9 ROS2 执行器、callback group 与实时边界 ⭐⭐¶

工程问题：ROS2 通信方便，但执行模型会影响延迟¶

ROS2 节点不是“收到消息就立即在独立线程执行”。回调由 executor 调度。 callback group 决定哪些回调可以并发。 QoS 决定队列、可靠性和历史策略。 DDS 实现、RMW 层和消息类型也会影响内存分配和拷贝路径。

实时路径如果不理解这些边界，很容易被非实时回调阻塞。

反面失败：控制回调和日志回调在同一个互斥 callback group¶

control_timer_callback  1kHz
diagnostics_callback    1Hz，但偶尔格式化大量字符串
same mutually exclusive callback group
single executor thread

诊断回调运行时，控制回调不能执行。即使诊断只有 1Hz，也会造成周期性长尾。

抽象不变量：实时回调要与非实时回调隔离¶

常见策略：

控制 timer 使用独立 callback group。
高优先级 executor 只服务实时相关回调。
日志、参数、可视化放入非实时 executor。
实时回调只读预准备数据，不做复杂分配。
跨边界使用无阻塞数据通道。

ROS2 QoS 与实时语义¶

QoS 不是只影响网络。它也影响队列积压和旧数据处理。

数据	常见 QoS 意图
最新位姿	保留最新，旧值可丢
控制命令	小队列，避免执行旧命令
传感器流	根据算法选择可靠或尽力而为
地图	可可靠传输，频率低
安全事件	不应轻易丢失

“可靠”不等于“实时”。可靠传输可能带来重传和队列积压。实时控制更常需要最新有效数据。

loaned message 与零拷贝边界¶

ROS2 支持 loaned message 的概念：发布者从中间件借用消息内存，填充后发布，减少拷贝。但能否真正零拷贝，取决于消息类型、RMW 实现、通信拓扑和版本支持。因此文档和代码应避免写成“用了 loaned message 就一定零拷贝”。

教学上应记住：

loaned message 是借用生命周期。
借来的内存不能随意长期持有。
不是所有消息类型和中间件都支持。
不支持时需要 fallback。
实际行为以当前 ROS2 和 RMW 官方文档为准。

代码验证：回调只做搬运，处理放到受控路径¶

class ImuBridge {
public:
    void onImuMessage(const ImuMsg& msg) {
        ImuSample sample;
        sample.ax = msg.linear_acceleration.x;
        sample.ay = msg.linear_acceleration.y;
        sample.az = msg.linear_acceleration.z;
        sample.gx = msg.angular_velocity.x;
        sample.gy = msg.angular_velocity.y;
        sample.gz = msg.angular_velocity.z;
        sample.stamp_ns = toNanoseconds(msg.header.stamp);

        if (!queue_.tryPush(sample)) {
            dropped_.fetch_add(1, std::memory_order_relaxed);
        }
    }

private:
    SpscRingBuffer<ImuSample, 512> queue_;
    std::atomic<std::uint64_t> dropped_{0};
};

回调只做固定成本转换和入队。复杂积分、优化和日志放到后续受控路径。这能降低 executor 抖动对传感器接收的影响。

⚠️ 编程陷阱：控制回调和诊断回调在同一个互斥 callback group 错误做法：把 1kHz 控制 timer 和 1Hz 诊断回调放在同一个 mutually exclusive callback group 中。现象：每秒有一次控制周期被诊断回调阻塞数毫秒，产生周期性 deadline miss。 根本原因：mutually exclusive callback group 保证同一时刻只有一个回调在执行。诊断回调运行期间，控制回调被排队等待。 正确做法：控制回调使用独立的 reentrant callback group 和专属高优先级 executor 线程。

💡 概念误区：认为 ROS2 "可靠 QoS" 等于 "实时安全" 新手想法："我把 QoS 设为 reliable，消息就不会丢了，系统就实时安全了。" 实际上：reliable QoS 保证传输可靠性，但可能引入重传和队列积压。如果消费者处理速度跟不上生产者，消息会堆积。对于控制命令，处理 20ms 前的旧命令比丢弃它更危险。实时控制更需要"最新有效数据"语义。

练习¶

[设计题] 为一个 ROS2 SLAM 节点设计 callback group 方案：控制 timer（1kHz）、LiDAR subscriber（10Hz）、参数 callback（偶发）、可视化 publisher（5Hz）。哪些应该隔离？哪些可以共用？
[分析题] 一个 ROS2 节点使用 reliable QoS 发布位姿。如果消费者处理速度突然降低，消息队列会如何增长？这对实时控制有什么影响？
[跨章综合题] 结合线程管理与互斥同步的线程管理和本节的 executor 模型，解释为什么 ROS2 的 MultiThreadedExecutor 不等于"每个回调一个独立线程"，以及这对实时回调的延迟有什么影响。

20.10 分支提示、热路径与冷路径 ⭐¶

工程问题：热路径应该连续、短小、可预测¶

C++20 提供 [[likely]] 和 [[unlikely]]。它们是分支概率提示。在一些编译器和架构上，提示可以影响代码布局。

典型场景：

跟踪状态通常正常，丢失是异常。
配准通常收敛，发散是异常。
队列通常未满，满是异常。
输入通常合法，非法是异常。

反面失败：到处添加 `[[likely]]`¶

分支提示不是性能装饰。如果标错热路径，可能降低性能。如果代码本来不在热点上，提示没有意义。如果编译器已经能从 profile-guided optimization 获得更准信息，手写提示反而可能过时。

抽象不变量：热路径保持短，冷路径移出主线¶

更重要的不是提示本身，而是代码结构：

if (valid) [[likely]] {
    fastPath();
} else {
    handleInvalidInput();
}

fastPath 应保持短小。冷路径可以记录日志、构造错误信息、触发重置。但这些不要混进每周期必经的主线。

代码验证：队列满作为冷路径¶

void pushImuOrDrop(const ImuSample& sample) {
    if (imu_queue_.tryPush(sample)) [[likely]] {
        return;
    }

    dropped_imu_.fetch_add(1, std::memory_order_relaxed);
    requestDegradedMode();
}

这里的语义是：

队列未满是热路径。
满队列是异常路径。
异常路径仍然有明确处理，不是静默失败。

💡 概念误区：认为 [[likely]] 能显著加速代码 新手想法："加了 [[likely]] 就能让分支预测更准确，性能应该有明显提升。" 实际上：现代 CPU 的分支预测器已经非常好，对于高频执行的分支，预测器很快就能学到正确模式。[[likely]] 的主要价值在于影响代码布局（让热路径代码连续），而不是直接改善分支预测。如果热点不在分支上，加这个提示完全没有效果。

练习¶

[代码题] 为一个实时 IMU 入队函数写出热路径/冷路径分离的结构，确保冷路径（队列满、降级处理）不在热路径的主线中。
[分析题] 如果 [[likely]] 标记错了方向——把实际不太可能的分支标记为 likely——会有什么后果？

20.11 实时日志与诊断：观察不能破坏被观察对象 ⭐⭐¶

工程问题：日志本身可能造成 deadline miss¶

实时循环中直接打印：

std::cout << "error: " << error << "\n";

可能触发：

格式化。
锁。
缓冲区刷新。
系统调用。
终端或文件 I/O。

这些都不适合硬实时路径。

反面失败：deadline miss 时同步写文件¶

if (elapsed > deadline) {
    logFile() << "miss " << elapsed.count() << "\n";
}

miss 已经发生。同步写文件可能让下一次 miss 更严重。诊断代码放大了故障。

抽象不变量：实时路径只记录固定成本事件¶

实时安全的诊断通常这样做：

原子计数器记录次数。
固定容量 ring buffer 记录简短事件。
后台线程异步取出并格式化。
满了就丢弃低优先级诊断。
严禁实时路径做字符串拼接和文件 I/O。

代码验证：固定事件记录¶

#include <cstdint>

enum class EventCode : std::uint16_t {
    DeadlineMiss,
    QueueFull,
    StateStale,
    SensorGap
};

struct RuntimeEvent {
    EventCode code = EventCode::DeadlineMiss;
    std::uint64_t stamp_ns = 0;
    std::int64_t value = 0;
};

class RuntimeEventLog {
public:
    void record(RuntimeEvent event) {
        if (!events_.tryPush(event)) {
            dropped_.fetch_add(1, std::memory_order_relaxed);
        }
    }

private:
    SpscRingBuffer<RuntimeEvent, 1024> events_;
    std::atomic<std::uint64_t> dropped_{0};
};

事件结构固定大小。实时路径只写数值。后台再把事件翻译成人可读日志。

实时日志和量子力学中的"观测者效应"有相似之处：测量行为本身会改变被测量的系统。在实时系统中，如果观测（日志）的成本太高，它会导致被观测的现象（deadline miss）更频繁地发生——甚至可能创造出原本不存在的 deadline miss。

⚠️ 编程陷阱：在 deadline miss 处理中调用 printf 或 std::cout 错误做法：if (elapsed > deadline) { std::cout << "MISS at " << timestamp << "\n"; } 现象：每次 miss 后下一个周期更容易 miss，形成"miss 连锁"。 根本原因：std::cout 可能触发 I/O 锁、缓冲区刷新和系统调用，耗时不可预测。在已经超时的周期末尾做这些操作，会让下一个周期的启动被进一步延迟。 正确做法：miss 时只递增原子计数器或写入固定大小 ring buffer，格式化和输出由非实时线程异步完成。

练习¶

[设计题] 设计一个实时安全的诊断系统：控制线程只写固定大小事件（枚举码 + 时间戳 + 一个整数值），后台线程每秒从 ring buffer 取出事件并格式化输出。
[分析题] spdlog 之类的异步日志库是否适合硬实时路径？分析它的队列入队操作是否可能触发分配或锁等待。

20.12 测量与 trace：实时问题必须可观测 ⭐⭐¶

工程问题：没有 trace，很难解释长尾¶

实时问题经常不是每次复现。只看终端输出很难定位。需要记录：

周期开始时间。
周期结束时间。
各阶段耗时。
队列长度或丢弃次数。
deadline miss。
当前模式。
输入时间戳间隔。

反面失败：只记录平均耗时¶

平均值会掩盖长尾：

1000 次中 999 次 100us
1 次 20ms
平均约 120us

平均值看起来安全。但那一次 20ms 可能已经让控制系统进入危险状态。

抽象不变量：实时监控必须看分位数和 miss¶

最小监控指标：

指标	说明
p50	常规耗时
p90	普通波动
p99	长尾
max	观测最大值
miss count	违反 deadline 次数
consecutive misses	连续 miss 风险
dropped count	队列溢出
stale state count	状态过期

代码验证：阶段计时¶

#include <array>
#include <chrono>
#include <cstdint>

enum class Stage : std::size_t {
    ReadSensors = 0,
    Estimate,
    Control,
    WriteActuator,
    Count
};

class StageTimer {
public:
    using Clock = std::chrono::steady_clock;

    void mark(Stage stage) {
        times_[static_cast<std::size_t>(stage)] = Clock::now();
    }

    std::int64_t durationUs(Stage a, Stage b) const {
        const auto ta = times_[static_cast<std::size_t>(a)];
        const auto tb = times_[static_cast<std::size_t>(b)];
        return std::chrono::duration_cast<std::chrono::microseconds>(tb - ta)
            .count();
    }

private:
    std::array<Clock::time_point, static_cast<std::size_t>(Stage::Count)> times_{};
};

这只是局部计时。系统级 trace 还需要结合平台工具。但从代码层面记录阶段边界，是定位长尾的第一步。

⚠️ 编程陷阱：只记录平均耗时掩盖长尾 错误做法：avg_ms = total_ms / count; 然后用平均值判断实时性。现象：平均值看起来安全，但系统每隔几秒仍然出现卡顿。 根本原因：平均值完全掩盖了分布特征。999 次 0.1ms + 1 次 20ms 的平均值是 0.12ms——看起来完全在预算内。 正确做法：至少记录 p50、p90、p99、max 和 miss count。用分位数而非平均值判断实时安全。

练习¶

[代码题] 为 StageTimer 增加"连续 miss 次数"统计。连续 3 次以上 miss 时触发一个降级标志。
[设计题] 如果需要记录一个 1kHz 控制循环 24 小时的每周期耗时，需要多少存储空间？如果使用 ring buffer 只保留最近 10 秒的数据，需要多少？

20.13 Mini SLAM Realtime Data Path¶

累积项目：本章新增模块¶

本章为 Mini SLAM 增加实时数据通道，在并行编程框架的并行处理层基础上，为 IMU 和 LiDAR 数据提供预分配、有界延迟的通信管道。项目进度：线程管理与互斥同步线程安全队列 -> 原子操作与内存模型原子发布 -> 并行编程框架并行执行策略 -> 实时约束与高性能数据传递实时数据通道。

工程问题：把实时规则落到一个小系统里¶

本章项目把前面规则整合成一个小型数据路径：

IMU callback 400Hz
    -> SPSC queue
    -> Frontend 周期处理
LiDAR callback 10Hz
    -> 预分配点云块
    -> scan processing
Estimator
    -> 最新状态发布
Monitor
    -> deadline miss 与丢弃统计

目标不是复刻完整 SLAM。目标是建立清楚的数据边界。

模块结构¶

mini_slam_realtime/
  include/
    imu_sample.hpp
    spsc_ring_buffer.hpp
    fixed_point_block.hpp
    latest_state.hpp
    runtime_metrics.hpp
    realtime_pipeline.hpp
  tests/
    test_spsc_order.cpp
    test_overflow_policy.cpp
    test_latest_state.cpp
    test_period_monitor.cpp

`imu_sample.hpp`¶

#pragma once

#include <cstdint>

struct ImuSample {
    double ax = 0.0;
    double ay = 0.0;
    double az = 0.0;
    double gx = 0.0;
    double gy = 0.0;
    double gz = 0.0;
    std::uint64_t stamp_ns = 0;
};

`runtime_metrics.hpp`¶

#pragma once

#include <atomic>
#include <cstdint>
#include <new>

struct alignas(std::hardware_destructive_interference_size) Counter {
    std::atomic<std::uint64_t> value{0};

    void increment() {
        value.fetch_add(1, std::memory_order_relaxed);
    }

    std::uint64_t load() const {
        return value.load(std::memory_order_relaxed);
    }
};

struct RuntimeMetrics {
    Counter imu_dropped;
    Counter cloud_dropped;
    Counter control_cycles;
    Counter deadline_misses;
    Counter stale_state;
};

`realtime_pipeline.hpp`¶

#pragma once

#include <chrono>
#include <optional>

class RealtimePipeline {
public:
    bool pushImu(const ImuSample& sample) {
        if (imu_queue_.tryPush(sample)) [[likely]] {
            return true;
        }
        metrics_.imu_dropped.increment();
        return false;
    }

    void controlCycle() {
        const auto begin = Clock::now();
        metrics_.control_cycles.increment();

        drainImu();
        const RobotState state = latest_state_.load();
        const Command command = computeCommand(state);
        writeCommand(command);

        const auto end = Clock::now();
        if (end - begin > control_deadline_) {
            metrics_.deadline_misses.increment();
        }
    }

    const RuntimeMetrics& metrics() const {
        return metrics_;
    }

private:
    using Clock = std::chrono::steady_clock;

    void drainImu() {
        constexpr int kMaxSamplesPerCycle = 32;
        for (int i = 0; i < kMaxSamplesPerCycle; ++i) {
            std::optional<ImuSample> sample = imu_queue_.tryPop();
            if (!sample) {
                break;
            }
            integrate(*sample);
        }
    }

    SpscRingBuffer<ImuSample, 512> imu_queue_;
    LatestState latest_state_;
    RuntimeMetrics metrics_;
    std::chrono::microseconds control_deadline_{1000};
};

这里故意给 drainImu() 加了每周期处理上限。没有上限时，控制周期的工作量会随队列积压增长；容量即使有限，也可能在一次循环里处理太多历史样本而错过 deadline。实时控制更关心“每周期最多做多少事”，而不是“最终是否把队列清空”。

这个骨架体现几个约束：

IMU 入队不分配。
控制周期只处理已有队列和最新状态。
溢出有计数。
deadline miss 有计数。
控制路径不写日志。

溢出测试¶

#include <cassert>

void testSpscOverflow() {
    SpscRingBuffer<int, 4> queue;
    assert(queue.tryPush(1));
    assert(queue.tryPush(2));
    assert(queue.tryPush(3));
    assert(!queue.tryPush(4));

    assert(queue.tryPop().value() == 1);
    assert(queue.tryPush(4));
}

容量为 4 的 ring buffer 可用槽位是 3。测试应明确这一点。

顺序测试¶

#include <cassert>

void testSpscOrder() {
    SpscRingBuffer<int, 8> queue;
    for (int i = 0; i < 7; ++i) {
        assert(queue.tryPush(i));
    }
    for (int i = 0; i < 7; ++i) {
        auto value = queue.tryPop();
        assert(value.has_value());
        assert(*value == i);
    }
    assert(!queue.tryPop().has_value());
}

实时数据结构的测试不只测成功路径。满、空、绕回、顺序都要测。

运行建议¶

在真实系统中，还应记录：

控制周期 p99。
IMU 队列最大占用。
LiDAR 块池最低剩余数量。
状态时间戳年龄。
连续 deadline miss 次数。
CPU 频率和温度。

这些指标能帮助判断容量是否合理、实时线程是否被干扰、非实时任务是否需要降载。

🔧 故障排查手册¶

现象	常见原因	检查方法	修复方向
平均很快但偶发超时	分配、I/O、调度抖动	记录 p99/max	移出实时路径
控制线程偶发卡住	等待低优先级锁	trace 锁等待	快照、try-lock、PI mutex
队列积压越来越长	消费者慢于生产者	记录队列占用	降采样、丢旧、减载
状态明显滞后	处理旧消息	检查时间戳年龄	最新状态通道
CPU 满载但 miss 增加	过度并行	查看线程数	统一并行预算
TSan 报 data race	缓冲生命周期不清	查看读写栈	明确所有权或加同步
使用对象池仍分配	内部容器扩容	重载分配统计	初始化 reserve
日志导致卡顿	实时路径格式化/I/O	关闭日志对比	固定事件队列
队列满后无反应	溢出策略缺失	注入高频输入	计数并降级
零拷贝后数据错乱	借用生命周期越界	审查持有时间	拷贝小对象或引用计数

20.14 本章小结¶

本章主线是：实时工程追求可预测，不只是追求更快。普通高性能优化关注平均耗时和吞吐。实时系统关注 deadline、WCET、jitter、阻塞上界和故障降级。

最重要的工程判断有六条：

实时路径不做无界操作。
所有容量、队列满策略和丢弃语义必须显式。
高优先级线程不应等待不可控低优先级工作。
小状态可以复制，大数据才需要复杂零拷贝。
ROS2 通信和执行器需要明确实时边界。
没有 p99、miss 和溢出统计，就没有实时调优。

从线程管理与互斥同步到实时约束与高性能数据传递，我们完成了并发与系统编程的第一轮闭环：

线程管理与互斥同步: 长期线程如何协作
原子操作与内存模型: 原子和内存模型如何定义可见性
并行编程框架: 并行框架如何提高吞吐
实时约束与高性能数据传递: 实时路径如何约束长尾

后续内存分配策略与pmr-缓存优化与数据布局会继续深入内存分配、缓存布局、pmr 和数据布局。那些内容会进一步解释为什么同样的算法，在不同内存组织下会产生完全不同的延迟曲线。

延伸阅读¶

Liu & Layland, "Scheduling Algorithms for Multiprogramming in a Hard-Real-Time Environment", JACM 1973 ⭐⭐——Rate Monotonic Analysis 的奠基论文，证明了 RM 调度的最优性和利用率上界。
Linux PREEMPT_RT 文档与 Linux 6.12+ 主线合并说明 ⭐⭐——从 Linux 6.12 开始，PREEMPT_RT 补丁的核心部分逐步合并进主线内核，理解中断线程化和 rt_mutex 的实现细节。
ROS2 executor、callback group、QoS 和 loaned message 官方文档 ⭐⭐——实际机器人系统中 ROS2 通信路径对实时性的影响，以当前版本文档为准。
C++ 并发标准文档（C++17/20） ⭐⭐⭐——std::atomic、memory_order、std::jthread（C++20）等并发原语的精确语义。
Burns & Wellings, "Real-Time Systems and Programming Languages", Addison-Wesley ⭐⭐⭐——实时系统理论教材，覆盖 WCET 分析、响应时间分析和优先级反转的形式化处理。
Mars Pathfinder 优先级反转事件分析（Glenn Reeves, JPL, "What Really Happened on Mars"） ⭐——NASA 工程师的第一手事件报告，是理解优先级反转工程后果的最佳入口。
平台性能分析工具文档：trace、perf、TSan、LTTng、ros2 tracing。