测试与调试基础设施¶

本章定位：把机器人 C++ 工程从“能跑一次”提升为“可重复验证、可定位退化、可回放故障、可度量性能”的工程系统。测试和调试不是项目尾声的补充，而是规控软件设计的一部分：每个模块都应该知道如何证明自己正确、如何暴露错误、如何在故障后复现实验。

前置自测¶

gtest 中 ASSERT_* 和 EXPECT_* 的区别是什么？
为什么 ASan 和 TSan 通常不能在同一次测试中同时启用？
微基准测试为什么不能替代端到端性能测试？
rosbag/mcap 回放为什么适合做回归测试？
火焰图中的宽度代表什么？

本章目标¶

学完本章后，你应该能为 C++ 控制、规划和 ROS2 节点建立测试金字塔。你应该能使用 sanitizer、perf、Tracy、Google Benchmark、PlotJuggler、rosbag2/mcap 和静态分析定位问题。你还应该能把调试结果转化为自动化检查，防止同类问题反复出现。

8.1 测试金字塔：从函数到系统 ⭐¶

这一节解决的问题是：机器人项目应该测试什么，以及每类测试承担什么职责。

动机：上机调试成本最高¶

机器人上机调试有三个特点：

复现成本高。
风险高。
变量多，难隔离。

因此越底层、越便宜的问题，越应该在离线测试中发现。上机测试应该验证真实硬件交互，而不是发现数组越界、符号错误或参数解析错误。

测试可以类比动力学建模中的层级。单元测试像验证一个公式。集成测试像验证模块耦合。系统测试像验证完整闭环。不要用系统测试替代公式验证。

测试层级¶

层级	对象	工具	典型判据
单元测试	函数、类、数学模块	gtest	解析解、边界条件
组件测试	求解器适配、控制核心	gtest/gmock	状态码、接口契约
ROS2 集成测试	节点图、launch	launch_testing	话题、服务、生命周期
回放测试	真实数据流	rosbag2/mcap	输出与基线一致
硬件在环	驱动和控制闭环	专用台架	安全约束满足
性能测试	耗时和内存	benchmark/perf/Tracy	p99、最大值、分配次数

反面失败：只靠上机测试¶

如果一个控制器的雅可比符号错了，上机后可能表现为抖动、力矩异常或跟踪差。这些现象不直接告诉你符号错误在哪里。如果有离线解析测试，错误会在几毫秒内定位到某个函数。

本质洞察：测试不是为了证明程序“没有 bug”，而是为了把故障定位成本从上机现场转移到可重复、可自动化、可缩小范围的环境中。

练习¶

为一个四足控制项目列出 5 个单元测试、3 个集成测试、2 个回放测试。
找一个只能上机发现的问题，思考如何构造离线近似测试提前暴露。
解释为什么随机仿真成功 100 次不能替代解析边界测试。

8.2 Google Test 与 gmock ⭐⭐¶

这一节解决的问题是：如何用 C++ 测试框架表达机器人模块的行为契约。

gtest 基本结构¶

#include <gtest/gtest.h>
#include <Eigen/Dense>

Eigen::Matrix3d skew(const Eigen::Vector3d& w) {
    Eigen::Matrix3d W;
    // 构造叉乘矩阵，使 W * v 等价于 w.cross(v)。
    W << 0.0, -w.z(), w.y(),
         w.z(), 0.0, -w.x(),
        -w.y(), w.x(), 0.0;
    return W;
}

TEST(SkewTest, MatchesCrossProduct) {
    const Eigen::Vector3d w(0.1, -0.2, 0.3);
    const Eigen::Vector3d v(1.0, 2.0, -1.0);
    // 测试数学性质，而不是只比较某个偶然的硬编码结果。
    EXPECT_TRUE((skew(w) * v).isApprox(w.cross(v), 1e-12));
}

这个测试把数学性质写成代码。比只检查某个硬编码结果更强，因为它验证了叉乘矩阵的本质关系。

Fixture¶

Fixture 适合多个测试共享构造成本。

#include <gtest/gtest.h>
#include <memory>

class Planner {
public:
    bool plan(double start, double goal) {
        // 示例规划器只允许向前规划，真实项目会替换为完整搜索逻辑。
        return goal >= start;
    }
};

class PlannerTest : public ::testing::Test {
protected:
    void SetUp() override {
        // 每个测试单独创建对象，避免测试之间共享隐式状态。
        planner_ = std::make_unique<Planner>();
    }

    std::unique_ptr<Planner> planner_;
};

TEST_F(PlannerTest, FindsForwardPath) {
    ASSERT_NE(planner_, nullptr);
    // ASSERT 先保证对象有效，EXPECT 再检查行为契约。
    EXPECT_TRUE(planner_->plan(0.0, 1.0));
}

参数化测试¶

#include <gtest/gtest.h>

class SaturationTest : public ::testing::TestWithParam<double> {};

double saturate(double x, double limit) {
    // 对称限幅函数，控制输出和力矩命令中很常见。
    if (x > limit) return limit;
    if (x < -limit) return -limit;
    return x;
}

TEST_P(SaturationTest, OutputWithinLimit) {
    const double input = GetParam();
    const double y = saturate(input, 2.0);
    // 不管输入多大，输出都必须落在物理限幅内。
    EXPECT_LE(y, 2.0);
    EXPECT_GE(y, -2.0);
}

INSTANTIATE_TEST_SUITE_P(Inputs,
                         SaturationTest,
                         ::testing::Values(-10.0, -2.0, 0.0, 1.0, 10.0));

gmock 模拟硬件接口¶

硬件接口不应在单元测试中访问真实电机。用 mock 可以验证控制器是否按契约调用接口。

#include <gmock/gmock.h>

class MotorInterface {
public:
    virtual ~MotorInterface() = default;
    // 硬件接口只暴露发送力矩命令的行为契约。
    virtual bool sendTorque(int joint, double torque) = 0;
};

class MockMotorInterface : public MotorInterface {
public:
    MOCK_METHOD(bool, sendTorque, (int joint, double torque), (override));
};

TEST(ControllerIoTest, SendsLimitedTorque) {
    MockMotorInterface motor;
    // 验证发送到 0 号关节的力矩已经被限制在安全范围内。
    EXPECT_CALL(motor, sendTorque(0, testing::AllOf(testing::Ge(-5.0), testing::Le(5.0))))
        .WillOnce(testing::Return(true));

    const double command = 4.0;
    // 单元测试不访问真实电机，只验证接口调用是否满足契约。
    EXPECT_TRUE(motor.sendTorque(0, command));
}

常见陷阱¶

类型	错误做法	现象	根本原因	正确做法
编程	测试依赖执行顺序	单独运行失败	测试间共享隐式状态	每个测试独立构造
概念	只测正常输入	边界上机才暴露	未覆盖极限条件	参数化边界值
工程	mock 过度指定调用细节	小重构导致测试全坏	测试实现而非行为	验证外部契约
思维	为覆盖率写无意义测试	覆盖高但 bug 多	没有断言性质	写数学和接口不变量

练习¶

为摩擦锥投影函数写参数化测试，覆盖零法向力、极大切向力和正常情况。
用 gmock 模拟一个 IMU 设备，测试状态估计模块在数据超时时的行为。
把一个只检查“函数不崩溃”的测试改成检查数学性质。

8.3 ROS2 集成测试与回放测试 ⭐⭐¶

这一节解决的问题是：节点之间的行为如何自动验证。

launch_testing¶

ROS2 集成测试可以启动真实节点图，检查话题、服务和进程退出。

import launch
import launch_ros.actions
import launch_testing
import pytest


@pytest.mark.launch_test
def generate_test_description():
    # 启动真实 ROS2 节点，测试节点图和进程行为。
    node = launch_ros.actions.Node(
        package='demo_nodes_cpp',
        executable='talker',
        output='screen'
    )
    # ReadyToTest 表示 launch 系统已准备好，测试进程可以开始断言。
    return launch.LaunchDescription([
        node,
        launch_testing.actions.ReadyToTest()
    ]), {'talker': node}

集成测试不应替代单元测试。它验证节点图连接、参数、生命周期和通信行为。

rosbag2/mcap 回放¶

真实传感器数据很难手工构造。录制一次典型场景，再反复回放，可以验证算法修改是否改变输出。

# 录制 IMU、关节状态和深度图，形成可重复回放输入。
ros2 bag record /imu /joint_states /camera/depth/image_rect_raw -o walking_case

# 使用 bag 内时间驱动节点，避免依赖当前墙钟时间。
ros2 bag play walking_case --clock

回放测试应固定：

输入数据。
参数文件。
随机种子。
输出比较规则。

不要要求浮点输出逐 bit 完全一致。应使用物理意义阈值，例如轨迹 RMSE、最大偏差、失败帧数。

PlotJuggler¶

PlotJuggler 适合快速观察时间序列。它的价值不是替代测试，而是帮助找到应写成测试的判据。例如发现力矩尖峰后，应把“力矩变化率不得超过阈值”写入回放测试。

练习¶

为一个状态估计节点写 launch test，检查输入 IMU 后是否发布 odom。
录制一段 rosbag，定义输出轨迹 RMSE 阈值，并写回放比较脚本。
用 PlotJuggler 找一个异常尖峰，把它转化为自动测试断言。

8.4 Sanitizer 与静态分析 ⭐⭐⭐¶

这一节解决的问题是：运行时和编译时工具能帮你抓住哪些 C++ 隐患。

Sanitizer 对比¶

工具	检测	开销	适合
ASan	越界、use-after-free、双重释放	约 2 到 3 倍	日常 CI
UBSan	未定义行为、溢出、非法移位	较低	日常 CI
TSan	数据竞争	约 5 到 15 倍	并发专项
MSan	未初始化读取	较高且依赖全链路	特定项目

ASan 和 TSan 通常分开跑。它们的运行时机制不适合叠加。

CMake 启用示例¶

# ENABLE_ASAN 用于在调试或 CI 中显式打开 AddressSanitizer。
option(ENABLE_ASAN "Enable AddressSanitizer" OFF)

if(ENABLE_ASAN)
  target_compile_options(controller_core PRIVATE
      # 保留栈帧信息，让 ASan 报告能显示更清楚的调用栈。
      -fsanitize=address
      -fno-omit-frame-pointer
  )
  target_link_options(controller_core PRIVATE
      # 链接阶段也必须启用 sanitizer 运行时。
      -fsanitize=address
  )
endif()

ASan 能发现的典型错误¶

#include <vector>

int useAfterFreeExample() {
    int* ptr = nullptr;
    {
        // data 的生命周期只在这个作用域内。
        std::vector<int> data(4, 1);
        ptr = data.data();
    }
    // data 已析构，ptr 悬垂。ASan 通常能报告 use-after-free。
    return ptr[0];
}

TSan 能发现的典型错误¶

#include <thread>

int shared_counter = 0;

void incrementBad() {
    for (int i = 0; i < 1000; ++i) {
        ++shared_counter;  // 数据竞争
    }
}

void runRace() {
    // 两个线程同时写同一个普通 int，TSan 会报告数据竞争。
    std::thread a(incrementBad);
    std::thread b(incrementBad);
    a.join();
    b.join();
}

clang-tidy 与 include-what-you-use¶

clang-tidy 适合发现可疑模式：

规则族	价值
`bugprone-*`	常见 bug 模式
`performance-*`	不必要拷贝和低效代码
`cppcoreguidelines-*`	资源和类型安全
`readability-*`	风格一致性

include-what-you-use 适合大型项目控制头文件依赖。编译慢往往不是编译器慢，而是头文件相互包含太重。

练习¶

写一个 use-after-free 示例，分别在普通构建和 ASan 构建运行。
写一个数据竞争示例，用 TSan 观察报告。
为一个 target 添加 clang-tidy，修复一个 performance-unnecessary-value-param 问题。

8.5 性能剖析：benchmark、perf、Tracy ⭐⭐⭐¶

这一节解决的问题是：性能问题如何从“感觉慢”变成可定位的证据。

Google Benchmark¶

微基准适合测单个函数或求解器。要避免编译器把计算优化掉。

#include <benchmark/benchmark.h>
#include <Eigen/Dense>

static void BM_MatrixMultiply(benchmark::State& state) {
    // 固定尺寸矩阵更接近控制器中常见的小矩阵计算。
    Eigen::Matrix<double, 12, 12> A = Eigen::Matrix<double, 12, 12>::Random();
    Eigen::Matrix<double, 12, 12> B = Eigen::Matrix<double, 12, 12>::Random();
    Eigen::Matrix<double, 12, 12> C;

    for (auto _ : state) {
        // noalias 告诉 Eigen 输出矩阵不与输入矩阵别名重叠。
        C.noalias() = A * B;
        // 防止编译器把结果计算优化掉。
        benchmark::DoNotOptimize(C.data());
    }
}

BENCHMARK(BM_MatrixMultiply)->Unit(benchmark::kNanosecond);
BENCHMARK_MAIN();

微基准只回答局部问题。控制器端到端延迟还包括缓存、线程、同步、通信和日志。

perf 与火焰图¶

# 记录调用栈采样数据。
perf record -g --call-graph dwarf ./controller_benchmark

# 在终端中查看热点函数和调用路径。
perf report

火焰图中横向宽度表示采样占比。宽的函数值得优先看。但采样占比高不一定就是错误，也可能是核心算法本来就应占主要时间。

Tracy¶

Tracy 适合实时观察多线程时间线。它能标记控制周期、求解器、消息回调、GPU 区间和内存分配。

// 概念示例：真实项目需包含 Tracy 头文件并启用编译选项。
void controlUpdate() {
    // 每个区间对应控制周期中的一个可观察阶段。
    // ZoneScopedN("control_update");
    // read_state();
    // solve_qp();
    // send_command();
}

性能指标¶

指标	含义	为什么重要
mean	平均耗时	粗略吞吐
median	中位数	典型情况
p95/p99	尾部延迟	观察偶发慢
max	最坏样本	实时 deadline
allocation count	分配次数	实时抖动来源
cache miss	缓存效率	数据布局问题

练习¶

为一个 QP 求解函数写 Google Benchmark，报告 p50、p99 和最大值。
用 perf 找出一个控制程序最耗时的三个函数，并判断是否符合预期。
用 Tracy 标记 ROS2 回调和控制线程，观察是否存在回调阻塞控制。

8.6 调试数值问题与控制故障 ⭐⭐⭐¶

这一节解决的问题是：优化、控制和估计的故障通常不是崩溃，而是数值变坏。

数值故障分类¶

故障	现象	常见原因
NaN/Inf	输出不可用	除零、sqrt 负数、未初始化
病态	迭代慢、解抖动	标度差、矩阵近奇异
不可行	求解器失败	约束冲突、初值不合理
饱和	控制效果差	参考超出能力
延迟	稳定性变差	线程阻塞或通信积压

数值断言¶

#include <Eigen/Dense>
#include <stdexcept>

void assertFinite(const Eigen::VectorXd& x, const char* name) {
    if (!x.allFinite()) {
        // 非实时测试中直接抛出异常，便于定位第一个非有限值来源。
        throw std::runtime_error(std::string(name) + " contains NaN or Inf");
    }
}

实时路径中不应抛异常。可以在非实时测试和调试构建中使用异常。实时控制中应返回状态码并进入降级策略。

记录最小复现数据¶

当控制器失败时，最有价值的数据不是完整日志海洋，而是能复现失败的最小输入包：

当前状态。
参考轨迹。
参数。
接触状态。
求解器状态码和残差。
时间戳和周期耗时。

这些数据应能喂给离线测试，复现同一次失败。

练习¶

给 QP 适配层增加 assertFinite 检查，并在实时版本改成状态码。
设计一个“最小复现包”结构体，要求可序列化到文件。
对一个不可行 QP 记录约束上下界，写离线测试复现求解器失败。

8.7 CI 中的质量门禁 ⭐⭐¶

这一节解决的问题是：哪些检查应该自动运行，哪些适合定期运行。

失败处理原则¶

CI 失败应提供足够信息定位问题。测试输出要包含随机种子、输入文件、参数文件和失败阈值。性能退化要显示历史基线和当前数值。

练习¶

设计一个 CI 配置，把 ASan 和 TSan 拆成两个 job。
为性能基准设置退化阈值：超过基线 20% 时失败。
解释为什么回放测试不适合每次提交都跑全部大数据集。

8.8 单元测试：把数学不变量写成可执行证据 ⭐⭐⭐¶

这一节解决的问题是：机器人算法的单元测试不应只检查“能运行”，而要检查数学性质、物理约束和边界行为。

动机：控制错误常常不是崩溃¶

普通后端程序出错时，常见现象是异常、错误码或服务不可用。机器人控制程序出错时，现象可能是轻微抖动、轨迹偏移、力矩尖峰或几分钟后才发散。这些问题很难靠“运行一次没崩”发现。

因此单元测试要抓住算法本质。对数学函数，测试它的代数性质。对控制函数，测试它的物理限幅。对规划函数，测试它的约束满足。对状态估计函数，测试它对时间戳、噪声和异常输入的响应。

不变量比硬编码结果更有价值¶

硬编码结果当然有用。例如一个小型 QP 的解析解可以直接写进测试。但很多机器人算法有更强的不变量。

模块	推荐不变量	示例
SO(3) 工具	旋转矩阵正交、行列式为 1	`R.transpose() * R = I`
雅可比	有限差分与解析 Jacobian 一致	误差小于阈值
摩擦锥	切向力不超过 `mu * fz`	投影后满足约束
轨迹插值	端点连续、速度有限	`p(0)=p0, p(T)=p1`
PID	饱和后输出不越界	力矩在限幅内
滤波器	协方差半正定	特征值非负

示例：SO(3) 指数映射测试¶

#include <gtest/gtest.h>
#include <Eigen/Dense>

Eigen::Matrix3d hat(const Eigen::Vector3d& w) {
    Eigen::Matrix3d W;
    // 构造李代数 so(3) 的反对称矩阵。
    W << 0.0, -w.z(), w.y(),
         w.z(), 0.0, -w.x(),
        -w.y(), w.x(), 0.0;
    return W;
}

Eigen::Matrix3d expSO3SmallAngle(const Eigen::Vector3d& w) {
    const Eigen::Matrix3d W = hat(w);
    // 小角度示例：保留到二阶，真实项目应使用稳定的 Rodrigues 公式。
    return Eigen::Matrix3d::Identity() + W + 0.5 * W * W;
}

TEST(SO3Test, RotationMatrixIsAlmostOrthogonal) {
    const Eigen::Vector3d w(1e-4, -2e-4, 3e-4);
    const Eigen::Matrix3d R = expSO3SmallAngle(w);

    // 正交性是旋转矩阵的核心不变量。
    const Eigen::Matrix3d should_be_identity = R.transpose() * R;
    EXPECT_TRUE(should_be_identity.isApprox(Eigen::Matrix3d::Identity(), 1e-8));

    // 行列式接近 1，防止映射退化成缩放或反射。
    EXPECT_NEAR(R.determinant(), 1.0, 1e-8);
}

这段测试没有问“某个矩阵元素是否等于 0.99999994”。它问的是“输出是否仍然是旋转”。这类测试更接近机器人学中的理论要求。

示例：有限差分验证 Jacobian¶

解析 Jacobian 容易写错符号。有限差分慢，但作为离线测试非常合适。

#include <gtest/gtest.h>
#include <Eigen/Dense>

Eigen::Vector2d residual(const Eigen::Vector2d& x) {
    // 示例残差：真实项目中可替换为重投影误差、质心误差或足端误差。
    return Eigen::Vector2d(x.x() * x.x() + x.y(), x.x() - std::sin(x.y()));
}

Eigen::Matrix2d analyticJacobian(const Eigen::Vector2d& x) {
    Eigen::Matrix2d J;
    // 手写解析 Jacobian，是最容易出现符号错误的地方。
    J << 2.0 * x.x(), 1.0,
         1.0, -std::cos(x.y());
    return J;
}

TEST(JacobianTest, MatchesFiniteDifference) {
    const Eigen::Vector2d x(0.3, -0.7);
    const double eps = 1e-6;
    Eigen::Matrix2d J_fd;

    for (int i = 0; i < 2; ++i) {
        Eigen::Vector2d dx = Eigen::Vector2d::Zero();
        dx[i] = eps;
        // 中心差分比前向差分误差更小，适合离线验证。
        J_fd.col(i) = (residual(x + dx) - residual(x - dx)) / (2.0 * eps);
    }

    EXPECT_TRUE(analyticJacobian(x).isApprox(J_fd, 1e-6));
}

有限差分测试不能替代解析推导。它的价值是让符号错误尽早暴露。在优化、动力学和状态估计中，这种测试非常常见。

浮点阈值怎么选¶

阈值不是越小越好。阈值要反映算法误差、传感器噪声和物理尺度。

场景	阈值思路
纯代数恒等式	接近机器精度，例如 `1e-12`
有限差分	受步长影响，常在 `1e-6` 到 `1e-8`
轨迹位置	按米或毫米设定，例如 `1e-3 m`
姿态误差	用角度或弧度阈值，例如 `1e-3 rad`
回放输出	按任务指标设定，例如 RMSE 和最大偏差

阈值应写进测试名称或失败输出。当测试失败时，读者要知道失败幅度和物理含义。

练习¶

为 SO(3) 对数映射写单元测试，检查 exp(log(R)) 是否回到原旋转。
为足端雅可比写有限差分测试，比较 Pinocchio 结果和数值扰动结果。
为力矩限幅函数设计三类输入：正常、越界、非有限值，并说明每类断言。

8.9 集成测试：验证模块契约而不是重复单元测试 ⭐⭐¶

这一节解决的问题是：多个模块接在一起时，应该测哪些跨边界行为。

单元测试和集成测试的分工¶

单元测试关心一个函数或一个类是否正确。集成测试关心模块之间的接口是否对齐。机器人系统中，集成错误常见于坐标系、单位、时间戳和生命周期。

错误类型	单元测试能否发现	集成测试关注点
函数公式符号错	能	通常不作为主目标
topic 名称错	不能	节点是否连接
参数单位错	部分能	上游配置与下游解释是否一致
坐标系错	部分能	TF、消息 frame_id 和算法约定
时间源错	部分能	sim time、bag time、steady clock 是否一致
生命周期顺序错	不能	节点启动、激活、退出顺序

集成测试不是更大的单元测试。它的价值在于发现边界处的误解。

ROS2 节点图测试目标¶

一个控制节点的集成测试可以检查：

启动后是否进入期望生命周期状态。
必需参数缺失时是否拒绝启动。
收到 /joint_states 后是否发布 /command。
输入时间戳落后时是否进入安全输出。
输出消息的 frame、维度和限幅是否正确。

这些检查不需要真实机器人。它们需要的是受控输入和可观察输出。

示例：伪造 joint_states 并等待 command¶

import rclpy
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64MultiArray


def test_controller_publishes_command(launch_service, proc_output):
    # 初始化 rclpy，用测试进程发布输入并订阅输出。
    rclpy.init()
    node = rclpy.create_node('controller_integration_test')

    received = []

    def on_command(msg):
        # 收集输出命令，后续断言维度和限幅。
        received.append(msg)

    pub = node.create_publisher(JointState, '/joint_states', 10)
    sub = node.create_subscription(Float64MultiArray, '/command', on_command, 10)

    msg = JointState()
    msg.name = ['hip', 'knee']
    msg.position = [0.1, -0.2]
    msg.velocity = [0.0, 0.0]

    # 多次发布输入，给节点图连接和回调处理留出时间。
    for _ in range(20):
        pub.publish(msg)
        rclpy.spin_once(node, timeout_sec=0.05)
        if received:
            break

    assert received, 'controller did not publish command'
    assert len(received[-1].data) == 2

    # 清理测试节点，避免影响同一进程中的其他测试。
    sub.destroy()
    node.destroy_node()
    rclpy.shutdown()

这类测试的重点不是控制律本身。控制律应由 C++ 单元测试验证。这里验证的是 ROS2 输入输出契约。

集成测试的边界¶

集成测试不应把系统变得不可维护。一个坏的集成测试会启动大量节点、依赖真实时间、要求网络稳定，还没有清晰断言。

好的集成测试	差的集成测试
输入短、断言明确	运行十分钟才知道失败
固定参数和时间源	依赖当前机器负载
只测接口契约	重复所有单元测试
失败输出包含 topic 和参数	只显示进程退出码

练习¶

为一个状态估计节点设计集成测试，检查 IMU 输入到 odom 输出的链路。
为控制节点设计“参数缺失必须启动失败”的测试。
找一个坐标系错误案例，说明单元测试和集成测试分别能覆盖哪一部分。

8.10 回放测试：把真实故障变成可重复输入 ⭐⭐⭐¶

这一节解决的问题是：如何用 rosbag2/mcap 把现场问题带回离线环境。

为什么回放测试重要¶

机器人故障往往发生在特定时间序列中。单个 IMU 样本、单个关节状态或单帧点云都看不出问题。问题来自组合：

某段路面造成足端打滑。
IMU 时间戳短暂跳变。
估计器输出延迟一帧。
控制器在接触切换时收到不一致状态。
求解器在某个约束组合下不可行。

这些组合难以手工构造。回放测试的价值就是保留真实输入序列。

回放测试的组成¶

组成	内容	目的
输入包	mcap 或 rosbag2	固定传感器和状态流
参数快照	YAML、URDF、控制参数	固定算法条件
时间源	`/clock` 或记录时间	固定调度语义
输出规则	RMSE、峰值、状态码、事件数	避免逐 bit 比较
失败片段	起止时间、topic 子集	缩短测试时间

回放测试不是把一小时数据全放进每次提交。更好的做法是从长日志中切出 10 秒到 60 秒的关键片段。每个片段对应一个明确风险。

从长 bag 截取关键片段¶

# 查看 bag 元信息，确认 topic、消息数量和时间范围。
ros2 bag info walking_failure

# 从完整数据中截取故障附近片段，减少回放测试耗时。
ros2 bag play walking_failure --start-offset 42.0 --duration 18.0 --clock

# 重新录制只包含关键 topic 的短片段。
ros2 bag record /imu /joint_states /foot_contacts /clock -o walking_failure_clip

实际项目常用专门脚本裁剪 mcap。教学阶段可以先用命令行复现流程。

输出比较不要逐 bit¶

机器人算法有浮点运算、线程调度和库版本差异。逐 bit 比较很脆弱。更合理的是比较物理指标。

输出类型	推荐指标
位姿轨迹	平移 RMSE、最大姿态误差
控制命令	最大力矩、力矩变化率、饱和次数
求解器	失败帧数、最大残差、平均迭代次数
状态估计	速度漂移、协方差范围、重置次数
规划器	碰撞次数、约束违反总时长

示例：回放输出比较脚本¶

import math
from dataclasses import dataclass


@dataclass
class ReplayMetric:
    rmse_position_m: float
    max_torque_nm: float
    solver_failure_count: int


def assert_replay_metric(metric: ReplayMetric) -> None:
    # 位置误差阈值应来自任务要求，而不是随意选择。
    assert metric.rmse_position_m < 0.03, metric

    # 力矩峰值不能超过硬件安全边界。
    assert metric.max_torque_nm < 23.0, metric

    # 回归测试中不允许求解器失败帧增加。
    assert metric.solver_failure_count == 0, metric


def compute_rmse(errors):
    # 使用均方根误差衡量整段轨迹质量。
    squared = [e * e for e in errors]
    return math.sqrt(sum(squared) / max(1, len(squared)))

这段代码没有展示如何读取 mcap。重点是输出判据的形态。真实项目可以用 rosbag2_py、MCAP Python 库或离线导出的 CSV。

回放测试分层¶

层级	数据量	频率	目标
smoke clip	5 到 15 秒	每次提交	快速发现明显退化
nightly set	多个 30 到 60 秒片段	每日	覆盖典型场景
release suite	大量场景	发布前	评估稳定性
incident case	故障片段	修复后长期保留	防止同类问题再次出现

回放数据也要版本化。大文件可以放对象存储或 Git LFS。关键是 CI 能准确下载到对应版本。

练习¶

录制一段 20 秒 mcap，选择 3 个 topic 作为回放输入，并说明为什么选它们。
为控制输出设计两个回放指标：一个约束安全，一个约束跟踪质量。
从一个长日志中定义“故障片段”的起止时间和保留 topic 集合。

8.11 性能测试：从平均耗时走向实时尾延迟 ⭐⭐⭐¶

这一节解决的问题是：控制系统真正关心的不是平均快，而是最坏情况下不要错过 deadline。

平均值的陷阱¶

一个 500 Hz 控制器的周期是 2 ms。如果平均耗时是 0.8 ms，但每 200 次出现一次 5 ms，机器人仍然可能抖动。实时控制更关心尾延迟、最大值和抖动来源。

指标	对实时控制的含义
mean	长期负载是否过高
p50	典型周期耗时
p95	常见慢路径
p99	偶发慢路径
max	deadline 风险
allocation count	动态分配风险
missed deadline	最直接的实时失败

性能测试要区分微基准和系统延迟。微基准适合优化矩阵函数或求解器接口。系统延迟适合检查线程、消息、锁、日志和内存分配。

控制周期统计器¶

#include <algorithm>
#include <chrono>
#include <vector>

class CycleTimer {
public:
    void addSample(double duration_ms) {
        // 记录每个控制周期耗时，离线测试中可以保留完整样本。
        samples_.push_back(duration_ms);
    }

    double percentile(double ratio) const {
        std::vector<double> sorted = samples_;
        std::sort(sorted.begin(), sorted.end());

        // 空样本返回 0，真实项目可改成错误状态。
        if (sorted.empty()) {
            return 0.0;
        }

        const std::size_t index =
            static_cast<std::size_t>(ratio * static_cast<double>(sorted.size() - 1));
        return sorted[index];
    }

    int countDeadlineMiss(double deadline_ms) const {
        int count = 0;
        for (const double sample : samples_) {
            // 超过 deadline 的周期应作为独立指标报告。
            if (sample > deadline_ms) {
                ++count;
            }
        }
        return count;
    }

private:
    std::vector<double> samples_;          // 测试工具可动态分配，实时路径不应这样写。
};

这段代码适合离线分析。实时路径中不要每周期 push_back。真实控制线程可以写入预分配环形缓冲区，或只记录聚合统计。

禁止热路径动态分配¶

很多控制抖动来自动态分配。例如每周期创建 std::vector、动态大小 Eigen 矩阵或格式化字符串。

#include <Eigen/Dense>

class FixedSizeController {
public:
    Eigen::Matrix<double, 12, 1> update(const Eigen::Matrix<double, 12, 1>& q) {
        // 固定尺寸矩阵在编译期确定大小，适合小型实时计算。
        Eigen::Matrix<double, 12, 1> tau;
        tau.noalias() = -kp_.cwiseProduct(q);
        return tau;
    }

private:
    Eigen::Matrix<double, 12, 1> kp_ =
        Eigen::Matrix<double, 12, 1>::Constant(20.0);  // 增益常量只初始化一次。
};

固定尺寸不是万能。如果机器人自由度运行时变化，动态矩阵不可避免。此时应在初始化阶段 resize，并在控制周期内复用内存。

trace 与性能测试的关系¶

benchmark 告诉你“某段代码多快”。 trace 告诉你“时间花在什么顺序和线程关系上”。两者结合才适合定位实时问题。

工具	适合问题
Google Benchmark	单函数或求解器耗时
perf	CPU 热点、调用栈采样
Tracy	多线程时间线、区间耗时、内存分配
LTTng/ros2_tracing	ROS2 executor、回调调度、DDS 相关事件
PlotJuggler	时间序列指标观察

练习¶

为 500 Hz 控制器定义 p99 和 max 阈值，并解释为什么不能只看 mean。
找一个每周期动态分配的代码路径，改成初始化阶段预分配。
用 Tracy 或 ros2_tracing 标记状态接收、估计、求解、命令发送四个阶段。

8.12 Sanitizer 深入：把 C++ 未定义行为挡在上机前 ⭐⭐⭐¶

这一节解决的问题是：不同 sanitizer 抓到的问题不同，CI 中应该如何分开配置。

ASan、UBSan、TSan 的互补性¶

ASan 抓内存越界和生命周期错误。 UBSan 抓未定义行为。 TSan 抓数据竞争。它们覆盖的错误类别不同，不能互相替代。

工具	典型机器人案例
ASan	传感器缓存越界、释放后仍被回调访问
UBSan	整数溢出、非法移位、除零、错误对齐
TSan	控制线程和通信线程同时访问状态
MSan	未初始化滤波状态被用于控制输出

CMake target 级 sanitizer¶

function(enable_asan_ubsan target_name)
    # 只给指定 target 添加 sanitizer，避免污染不兼容的第三方库。
    target_compile_options(${target_name}
        PRIVATE
            -fsanitize=address,undefined
            -fno-omit-frame-pointer
            -g
    )

    # 链接选项必须与编译选项一致。
    target_link_options(${target_name}
        PRIVATE
            -fsanitize=address,undefined
    )
endfunction()

# 示例：给控制核心测试 target 启用 ASan 和 UBSan。
enable_asan_ubsan(controller_core_test)

推荐先给测试 target 开 sanitizer。如果给所有库和可执行文件统一开启，需要确保第三方库兼容。某些预编译库不带 sanitizer 信息，报告可能不完整。

TSan 示例：双缓冲替代共享写¶

错误做法是控制线程和 ROS2 回调线程同时读写同一个状态对象。更稳妥的做法是使用互斥锁、原子指针或双缓冲。

#include <mutex>
#include <optional>

struct RobotState {
    double stamp = 0.0;                    // 状态时间戳。
    double q0 = 0.0;                       // 示例关节位置。
};

class StateBuffer {
public:
    void writeFromCallback(const RobotState& state) {
        std::lock_guard<std::mutex> lock(mutex_);
        latest_ = state;                   // 回调线程写入最新状态。
    }

    std::optional<RobotState> readFromControl() {
        std::lock_guard<std::mutex> lock(mutex_);
        return latest_;                    // 控制线程读取快照，避免数据竞争。
    }

private:
    std::mutex mutex_;
    std::optional<RobotState> latest_;
};

互斥锁会带来阻塞风险。实时控制中可以进一步使用无锁环形缓冲或单生产者单消费者队列。但无论选择哪种机制，都应该用 TSan 或压力测试验证没有数据竞争。

sanitizer CI 拆分¶

name: sanitizer

on:
  push:
  pull_request:

jobs:
  asan_ubsan:
    runs-on: ubuntu-latest
    steps:
      - uses: actions/checkout@v4
      - name: Configure ASan UBSan
        # ASan 和 UBSan 常一起跑，覆盖内存和未定义行为。
        run: cmake -S . -B build-asan -DENABLE_ASAN=ON -DENABLE_UBSAN=ON
      - name: Test ASan UBSan
        # sanitizer 报告会让测试进程以失败状态退出。
        run: cmake --build build-asan && ctest --test-dir build-asan --output-on-failure

  tsan:
    runs-on: ubuntu-latest
    steps:
      - uses: actions/checkout@v4
      - name: Configure TSan
        # TSan 开销较大，通常单独 job 运行。
        run: cmake -S . -B build-tsan -DENABLE_TSAN=ON
      - name: Test TSan
        # 并发测试应增加重复次数，提高触发概率。
        run: cmake --build build-tsan && ctest --test-dir build-tsan --output-on-failure

ASan 和 TSan 通常分开。它们的运行时插桩机制不适合同时使用。这不是习惯问题，而是工具实现和开销决定的。

练习¶

为一个测试 target 添加 ASan+UBSan 配置，不影响 release 可执行文件。
写一个共享状态数据竞争示例，用 TSan 观察报告，再用互斥锁修复。
解释为什么 sanitizer 构建不应作为性能基准的依据。

8.13 仿真测试：在真实硬件之前验证闭环行为 ⭐⭐⭐¶

这一节解决的问题是：仿真测试应该验证什么，以及它和回放测试、硬件测试的边界在哪里。

仿真测试的角色¶

仿真不是现实的完全替代。仿真测试的价值在于低成本覆盖闭环逻辑。它可以反复验证规划、控制、状态估计和安全策略的交互。它也可以制造真实硬件上危险或昂贵的场景。

测试类型	输入来源	是否闭环	典型用途
单元测试	人工构造	否	数学与接口
回放测试	真实日志	通常否	回归和故障复现
仿真测试	仿真环境	是	闭环行为和场景覆盖
硬件测试	真实世界	是	最终验证

仿真测试要避免两个极端。一个极端是完全不信仿真，所有问题都上机找。另一个极端是过度相信仿真，以为仿真通过就等于真实通过。正确做法是明确仿真验证的假设。

仿真场景设计¶

仿真场景应覆盖机器人任务中的关键风险：

平地直行，验证基本闭环。
速度阶跃，验证跟踪和限幅。
小障碍，验证摆腿和接触切换。
低摩擦地面，验证打滑处理。
传感器延迟，验证估计和控制鲁棒性。
短时消息丢失，验证安全降级。

每个场景都要有可量化判据。只看仿真画面很容易漏掉慢性退化。

仿真判据表¶

场景	判据
平地直行	横向偏差小于阈值，未摔倒
速度阶跃	超调和稳态误差在范围内
低摩擦	接触力不违反摩擦锥过多
障碍跨越	足端最低高度满足 clearance
延迟注入	控制周期未大量超时
丢包	输出进入保持或降级策略

pytest 驱动仿真命令¶

import subprocess
from pathlib import Path


def test_flat_walk_simulation(tmp_path: Path):
    output = tmp_path / 'flat_walk_metrics.json'

    cmd = [
        'ros2',
        'launch',
        'robot_sim',
        'flat_walk.launch.py',
        f'metrics_file:={output}',
    ]

    # 运行仿真并设置超时，防止 CI 被卡住。
    result = subprocess.run(cmd, timeout=120, text=True, capture_output=True)

    # 进程失败时打印标准输出和错误输出，便于定位 launch 或节点问题。
    assert result.returncode == 0, result.stdout + result.stderr

    # 指标文件由仿真节点写出，后续测试读取并断言。
    assert output.exists()

这只是外层驱动。更关键的是仿真程序要输出结构化指标。不要只依赖终端日志。

仿真指标 JSON 示例¶

// 教学注释版指标文件；真实 JSON 指标不应包含注释行。
{
  "duration_s": 20.0,
  "fell_down": false,
  "max_base_roll_rad": 0.18,
  "max_torque_nm": 18.5,
  "deadline_miss_count": 0,
  "mean_tracking_error_m": 0.024
}

读取指标后，测试可以直接检查：

import json
from pathlib import Path


def assert_sim_metrics(metrics_file: Path):
    # 读取仿真输出的结构化指标。
    metrics = json.loads(metrics_file.read_text())

    # 不允许摔倒，这是最基本的安全判据。
    assert not metrics['fell_down']

    # 姿态、力矩和实时性都应在任务定义的阈值内。
    assert metrics['max_base_roll_rad'] < 0.35
    assert metrics['max_torque_nm'] < 23.0
    assert metrics['deadline_miss_count'] == 0

练习¶

为四足平地行走设计 5 个仿真指标，并说明每个指标对应的风险。
把一个只看 RViz 画面的仿真验证改成输出 JSON 指标并由 pytest 断言。
设计一个传感器延迟注入场景，说明预期控制器应如何降级。

8.14 机器人故障复现：从现场现象到最小输入包 ⭐⭐⭐¶

这一节解决的问题是：当机器人现场出现问题，如何把“现象描述”转化为可离线复现的测试。

故障复现的目标¶

现场问题常以模糊语言出现：

“机器人偶尔抖一下。”
“转弯时后腿会踢地。”
“跑一会儿求解器失败。”
“某个版本感觉变慢。”

这些描述不能直接修复。需要把它转化为输入、状态、参数和判据。

故障复现的目标是得到一个最小输入包。它能在离线环境中触发同一类失败。它越小，定位越快。

最小输入包内容¶

数据	说明
状态快照	`q`、`dq`、base pose、接触状态
参考输入	期望速度、轨迹、任务权重
参数快照	控制增益、限幅、URDF/SRDF 版本
求解器数据	Hessian、梯度、约束上下界、状态码
时间信息	时间戳、周期耗时、延迟
外部事件	接触切换、急停、模式切换

最小输入包不一定包含完整 bag。对于 QP 不可行，保存矩阵和上下界往往比保存所有传感器流更直接。对于状态估计漂移，保存关键传感器时间序列更合适。

C++ 复现包结构¶

#include <Eigen/Dense>
#include <string>
#include <vector>

struct ContactState {
    std::string foot_name;                 // 足端名称，例如 FL、FR、RL、RR。
    bool in_contact = false;               // 当前足端是否处于接触状态。
};

struct QpDebugPacket {
    Eigen::VectorXd q;                     // 机器人广义位置。
    Eigen::VectorXd dq;                    // 机器人广义速度。
    Eigen::VectorXd gradient;              // QP 线性项。
    Eigen::VectorXd lower_bound;           // 约束下界。
    Eigen::VectorXd upper_bound;           // 约束上界。
    std::vector<ContactState> contacts;    // 接触状态快照。
    double stamp_s = 0.0;                  // 数据时间戳。
    int solver_status = 0;                 // 求解器返回状态码。
};

这个结构还不包含矩阵。真实 QP 复现包需要保存 Hessian 和约束矩阵。如果矩阵很大，应使用稀疏格式。

失败快照写入原则¶

只在失败或接近失败时写入。
写入动作不阻塞实时线程。
文件名包含时间、状态码和场景。
包内记录参数版本和代码版本。
离线测试能直接读取该包并复现失败。

实时线程不要直接写大文件。它可以把快照放入无锁队列或预分配缓冲区，由非实时线程落盘。

离线复现测试¶

#include <gtest/gtest.h>
#include <string>

QpDebugPacket loadDebugPacket(const std::string& path) {
    // 示例函数：真实项目应从 JSON、NPZ、protobuf 或自定义二进制读取。
    (void)path;
    QpDebugPacket packet;
    packet.solver_status = -1;
    return packet;
}

bool replayQpSolve(const QpDebugPacket& packet) {
    // 示例函数：真实项目在这里重建 QP 并调用求解器。
    return packet.solver_status == 0;
}

TEST(QpReplayTest, ReproducesInfeasibleContactSwitch) {
    const QpDebugPacket packet = loadDebugPacket("data/contact_switch_failure.json");

    // 测试名称说明故障类型，便于长期保留为回归用例。
    EXPECT_FALSE(replayQpSolve(packet));
}

修复完成后，这个测试的期望可能会改变。如果修复目标是“同样输入不再不可行”，断言应变为成功并检查残差。关键是保留输入包，使同类问题不再只存在现场记忆中。

故障树¶

面对“机器人抖动”，可以按故障树拆解：

分支	检查数据
参考突变	期望速度、轨迹加速度、模式切换时间
状态突变	IMU、关节速度、接触估计
求解器异常	状态码、残差、约束上下界
输出异常	力矩峰值、力矩变化率、饱和次数
调度异常	控制周期、回调延迟、锁等待

每个分支都对应一种测试。参考突变可以用单元测试和回放测试。状态突变可以用回放测试。求解器异常可以用最小 QP 复现包。调度异常需要 trace 和性能测试。

练习¶

为“接触切换时 QP 不可行”设计最小复现包字段。
把“机器人偶尔抖一下”拆成至少四个可验证假设。
设计一个离线复现测试，使它能读取失败快照并重新调用控制器一步。

8.15 Trace：把时间线变成可解释的调试材料 ⭐⭐¶

这一节解决的问题是：当系统没有崩溃但变慢、阻塞或顺序异常时，如何看清发生了什么。

日志、指标和 trace 的区别¶

日志适合记录事件。指标适合记录数值趋势。 trace 适合记录时间线。

工具形态	回答的问题
日志	发生了什么事件
指标	数值是否变坏
trace	哪个线程在什么时候做了什么
profile	CPU 时间主要花在哪里

控制系统常需要 trace。例如 p99 延迟突然变差，日志只告诉你“周期超时”。 trace 可以告诉你超时之前控制线程是否等锁、是否被回调占用、是否触发内存分配。

控制周期 trace 区间¶

void updateControlLoop() {
    // ZoneScopedN("control_loop");

    {
        // ZoneScopedN("read_state");
        // 从状态缓冲区读取一份快照。
    }

    {
        // ZoneScopedN("state_estimation");
        // 执行状态估计预测和更新。
    }

    {
        // ZoneScopedN("solve_mpc");
        // 求解 MPC 或 QP，重点观察尾延迟。
    }

    {
        // ZoneScopedN("send_command");
        // 输出命令到硬件接口或仿真接口。
    }
}

这些注释形式的宏表达了 trace 应该标记的位置。真实项目中可替换为 Tracy、LTTng 或自研 trace 宏。关键是区间名称稳定，便于跨版本比较。

trace 结果如何转成测试¶

trace 不应只停留在图形界面观察。当你发现问题后，应提炼成自动化指标。

trace 发现	自动化指标
`solve_mpc` 偶发 8 ms	p99 和 max 阈值
`read_state` 等锁	锁等待时间或无锁缓冲测试
回调堆积	消息队列长度或处理延迟
日志格式化占用时间	热路径禁止同步日志
内存分配出现在控制周期	allocation count 阈值

练习¶

为控制周期设计 5 个 trace 区间，并说明每个区间应覆盖哪些代码。
从一次 trace 中选择一个慢区间，设计对应的自动化性能指标。
解释为什么 trace 区间名称应保持稳定，而不是随意改动。

8.16 Fuzzing 测试：让机器发现你想不到的输入 ⭐⭐⭐¶

这一节解决的问题是：如何用模糊测试发现人工编写的测试用例无法覆盖的边界错误。

为什么机器人代码需要 fuzzing¶

传统单元测试的输入由开发者手工选择。即使覆盖了正常、边界和异常情况，仍可能遗漏某些组合。机器人代码的输入空间非常大：关节角有连续范围、IMU 数据有时间序列依赖、参数有复杂的有效域。fuzzing 自动生成大量随机或半随机输入，试图触发崩溃、断言失败或 sanitizer 报告。

fuzzing 可以类比在仿真中随机化初始条件。单元测试像精心挑选的几个仿真场景，fuzzing 像让仿真器在整个状态空间中随机探索。仿真器偶尔会发现一个开发者从未想到的初始姿态让控制器崩溃——fuzzing 对代码做的是同样的事情。

测试方式	输入来源	覆盖策略	发现的问题类型
单元测试	手工选择	精确覆盖已知风险	已知的边界和不变量
参数化测试	枚举或随机抽样	覆盖多个典型点	已知参数空间中的错误
Fuzzing	自动生成	覆盖引导探索	未知的崩溃和未定义行为

libFuzzer 在机器人代码中的应用¶

libFuzzer 是 LLVM 内置的覆盖引导模糊测试引擎。它通过编译器插桩追踪代码覆盖率，优先保留能覆盖新代码路径的输入。与 ASan 和 UBSan 结合使用时，libFuzzer 不仅能发现崩溃，还能发现内存越界和未定义行为。

#include <cstddef>
#include <cstdint>
#include <cstring>
#include <Eigen/Dense>

// 被测函数：从字节流中解析关节角度并计算正运动学。
Eigen::Vector3d forwardKinematics(const double* q, int nq) {
    if (nq < 2) {
        return Eigen::Vector3d::Zero();
    }
    // 简化的二连杆正运动学。
    const double l1 = 0.3;
    const double l2 = 0.25;
    double x = l1 * std::cos(q[0]) + l2 * std::cos(q[0] + q[1]);
    double y = l1 * std::sin(q[0]) + l2 * std::sin(q[0] + q[1]);
    return Eigen::Vector3d(x, y, 0.0);
}

extern "C" int LLVMFuzzerTestOneInput(const uint8_t* data, size_t size) {
    // 需要至少 2 个 double（16 字节）。
    if (size < sizeof(double) * 2) {
        return 0;
    }

    const int nq = static_cast<int>(size / sizeof(double));
    // 把随机字节解释为 double 数组。
    double q[16] = {};
    const int n = (nq > 16) ? 16 : nq;
    std::memcpy(q, data, n * sizeof(double));

    Eigen::Vector3d result = forwardKinematics(q, n);

    // 断言：正运动学结果必须有限。
    if (!result.allFinite()) {
        __builtin_trap();
    }

    return 0;
}

编译时需要同时启用 fuzzer 和 sanitizer：

# 编译 fuzz target，同时启用覆盖引导和 ASan。
clang++ -g -fsanitize=fuzzer,address,undefined \
    -I/usr/include/eigen3 \
    fuzz_fk.cpp -o fuzz_fk

# 运行 fuzzer，自动探索输入空间。
./fuzz_fk -max_total_time=60

本质洞察：fuzzing 不是"更多的单元测试"，而是一种根本不同的测试范式。单元测试回答"这个特定输入的输出对不对"，fuzzing 回答"有没有某个输入能让程序崩溃"。前者验证预期行为，后者探索未知盲区。两者互补，不可替代。

适合 fuzzing 的机器人模块¶

不是所有模块都适合 fuzzing。适合的是那些接受外部输入、有复杂内部逻辑、且可能在异常输入下崩溃的模块。

模块	fuzzing 适合度	fuzzing 目标
消息解析（PointCloud2、JointState）	高	畸形消息不崩溃
参数验证	高	非法参数不导致未定义行为
轨迹插值	中	退化输入不产生 NaN
QP 矩阵构造	中	极端状态不导致越界
正运动学/逆运动学	中	奇异位形不崩溃
ROS2 服务处理	高	畸形请求不影响服务

常见陷阱¶

类型	错误做法	现象	根本原因	正确做法
工程	fuzzing 运行时间太短	覆盖不足	复杂路径需要时间探索	至少运行几分钟到几小时
编程	fuzz target 中做大量 IO	吞吐极低	fuzzer 需要高速迭代	fuzz target 应纯计算
概念	认为 fuzzing 替代单元测试	不变量未检查	fuzzing 只找崩溃	fuzzing 和单元测试互补
思维	不用 sanitizer 配合 fuzzing	只发现崩溃	内存错误不一定崩溃	始终搭配 ASan+UBSan

练习¶

为一个轨迹插值函数写 libFuzzer target，断言输出在物理范围内。
对一个参数解析函数做 10 分钟 fuzzing，报告发现的 bug 数量和类型。
解释为什么 fuzzing 适合"防御性编程"代码的验证，而不适合替代数学不变量测试。

8.17 Google Benchmark 深入：控制器微基准的正确姿势 ⭐⭐⭐¶

这一节解决的问题是：如何避免微基准测试的常见陷阱，得到可信赖的性能数据。

微基准的五个陷阱¶

微基准测试看起来简单，但有五个常见错误会让结果完全不可信。

陷阱	现象	原因	修复
编译器把计算优化掉	耗时接近零	结果未被使用	`DoNotOptimize`
缓存预热不一致	第一次快后面慢	冷缓存 vs 热缓存	使用 fixture 预热
测量粒度太粗	结果波动大	单次操作太快	增加迭代或改用纳秒单位
系统负载干扰	结果不可重复	后台进程占用 CPU	固定 CPU 频率和隔离核
只看平均不看分布	尾延迟被隐藏	偶发慢路径	报告 p50/p95/p99/max

高级 Benchmark 用法¶

Google Benchmark 支持多种高级用法，适合机器人性能测试。

#include <benchmark/benchmark.h>
#include <Eigen/Dense>

class ControllerBenchmark : public benchmark::Fixture {
public:
    void SetUp(const benchmark::State&) override {
        // 预分配所有内存，避免在基准循环中 malloc。
        J_.setRandom();
        W_.setIdentity();
        H_.setZero();
    }

protected:
    Eigen::Matrix<double, 6, 12> J_;
    Eigen::Matrix<double, 6, 6> W_;
    Eigen::Matrix<double, 12, 12> H_;
};

BENCHMARK_DEFINE_F(ControllerBenchmark, HessianAssembly)(benchmark::State& state) {
    for (auto _ : state) {
        H_.setZero();
        const Eigen::Matrix<double, 12, 6> JtW = J_.transpose() * W_;
        H_.noalias() += JtW * J_;
        benchmark::DoNotOptimize(H_.data());
    }
}

BENCHMARK_REGISTER_F(ControllerBenchmark, HessianAssembly)
    ->Unit(benchmark::kNanosecond)
    ->Iterations(100000);

自定义统计量¶

默认 Google Benchmark 只报告 mean 和 stddev。对实时控制，更需要分位数。可以通过自定义计数器和手动收集样本来报告尾延迟。

#include <benchmark/benchmark.h>
#include <chrono>
#include <vector>
#include <algorithm>

static void BM_WithPercentiles(benchmark::State& state) {
    std::vector<double> durations;
    durations.reserve(100000);

    Eigen::Matrix<double, 12, 12> A = Eigen::Matrix<double, 12, 12>::Random();
    Eigen::Matrix<double, 12, 1> b = Eigen::Matrix<double, 12, 1>::Random();
    Eigen::Matrix<double, 12, 1> x;

    for (auto _ : state) {
        auto start = std::chrono::high_resolution_clock::now();
        x = A.ldlt().solve(b);
        benchmark::DoNotOptimize(x.data());
        auto end = std::chrono::high_resolution_clock::now();

        durations.push_back(
            std::chrono::duration<double, std::nano>(end - start).count());
    }

    std::sort(durations.begin(), durations.end());
    if (!durations.empty()) {
        const auto n = durations.size();
        // 通过自定义计数器报告分位数。
        state.counters["p50_ns"] = durations[n * 50 / 100];
        state.counters["p95_ns"] = durations[n * 95 / 100];
        state.counters["p99_ns"] = durations[n * 99 / 100];
        state.counters["max_ns"] = durations.back();
    }
}

BENCHMARK(BM_WithPercentiles)->Unit(benchmark::kNanosecond)->Iterations(10000);

参数化基准¶

机器人代码中常需要测试不同维度下的性能变化。Google Benchmark 的 Arg 和 Range 支持参数化。

#include <benchmark/benchmark.h>
#include <Eigen/Dense>

static void BM_DenseMultiply(benchmark::State& state) {
    const int n = state.range(0);
    Eigen::MatrixXd A = Eigen::MatrixXd::Random(n, n);
    Eigen::MatrixXd B = Eigen::MatrixXd::Random(n, n);
    Eigen::MatrixXd C(n, n);

    for (auto _ : state) {
        C.noalias() = A * B;
        benchmark::DoNotOptimize(C.data());
    }

    // 报告每秒浮点运算量，便于跨平台比较。
    state.SetItemsProcessed(state.iterations());
    state.counters["dim"] = n;
}

// 测试 3x3 到 100x100 的矩阵乘法性能曲线。
BENCHMARK(BM_DenseMultiply)->Arg(3)->Arg(6)->Arg(12)->Arg(24)->Arg(48)->Arg(100);

常见陷阱¶

类型	错误做法	现象	根本原因	正确做法
编程	不用 `DoNotOptimize`	耗时为零	编译器消除死代码	标记结果为不可优化
编程	在循环内分配内存	分配时间被计入	测量的不是纯计算	SetUp 中预分配
概念	微基准代替系统基准	局部快整体慢	缓存、线程、通信被忽略	微基准 + 系统 trace
工程	CI 上的微基准结果波动大	无法检测退化	共享 runner 负载不稳定	使用专用机器或相对比较

练习¶

为 QP 求解器写 fixture benchmark，预分配所有矩阵，报告 p99 耗时。
用参数化 benchmark 测试 Cholesky 分解在维度 6、12、24、50 下的性能曲线。
对比同一函数在 -O0、-O2、-O3 -march=native 下的 benchmark 结果。

8.18 Mock 和 Fake 在 ROS2 节点测试中的使用 ⭐⭐¶

这一节解决的问题是：如何在不启动真实硬件或完整 ROS2 系统的情况下测试节点行为。

Mock、Fake 和 Stub 的区别¶

测试替身（test double）有三种主要形态，它们在机器人测试中扮演不同角色。

类型	定义	机器人例子	验证方式
Stub	返回固定值	IMU 传感器返回固定加速度	不验证调用
Fake	有简化实现	内存中的参数服务替代 ROS2 参数	功能正确但简化
Mock	可验证调用	验证控制器是否调用了电机接口	检查调用次数和参数

在 ROS2 节点测试中，这三种替身常常组合使用。传感器输入用 Stub 或 Fake 提供固定数据流；硬件接口用 Mock 验证控制器的输出行为；参数服务用 Fake 替代真实 ROS2 参数服务器。

Fake 传感器节点¶

在集成测试中，可以用一个简化的"假"传感器节点替代真实传感器。这个节点发布固定或预录的传感器数据。

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
import math


class FakeImuNode(Node):
    """发布固定 IMU 数据的测试替身节点。"""

    def __init__(self):
        super().__init__('fake_imu')
        self.pub_ = self.create_publisher(Imu, '/imu', 10)
        # 100 Hz 发布，模拟真实 IMU 频率。
        self.timer_ = self.create_timer(0.01, self.publish_imu)
        self.t_ = 0.0

    def publish_imu(self):
        msg = Imu()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'imu_link'
        # 静止状态：加速度仅有重力分量。
        msg.linear_acceleration.z = 9.81
        # 轻微角速度，模拟微小扰动。
        msg.angular_velocity.x = 0.001 * math.sin(self.t_)
        self.t_ += 0.01
        self.pub_.publish(msg)

gmock 验证硬件接口¶

回顾 8.2 节的 gmock 用法，这里展示一个更贴近实际的机器人硬件接口 mock 模式。

#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <Eigen/Dense>

class HardwareInterface {
public:
    virtual ~HardwareInterface() = default;
    // 发送关节力矩命令。
    virtual bool sendJointTorques(const Eigen::Matrix<double, 12, 1>& tau) = 0;
    // 读取关节状态。
    virtual bool readJointState(Eigen::Matrix<double, 12, 1>* q,
                                Eigen::Matrix<double, 12, 1>* dq) = 0;
    // 设置安全限幅。
    virtual void setTorqueLimit(double limit) = 0;
};

class MockHardware : public HardwareInterface {
public:
    MOCK_METHOD(bool, sendJointTorques,
                (const Eigen::Matrix<double, 12, 1>&), (override));
    MOCK_METHOD(bool, readJointState,
                (Eigen::Matrix<double, 12, 1>*, Eigen::Matrix<double, 12, 1>*),
                (override));
    MOCK_METHOD(void, setTorqueLimit, (double), (override));
};

TEST(ControllerHardwareTest, InitializeSetsLimitBeforeSending) {
    MockHardware hw;

    // 验证调用顺序：必须先设限幅，再发送力矩。
    {
        testing::InSequence seq;
        EXPECT_CALL(hw, setTorqueLimit(testing::DoubleNear(20.0, 0.1)));
        EXPECT_CALL(hw, sendJointTorques(testing::_))
            .WillOnce(testing::Return(true));
    }

    // 模拟控制器初始化流程。
    hw.setTorqueLimit(20.0);
    Eigen::Matrix<double, 12, 1> tau = Eigen::Matrix<double, 12, 1>::Zero();
    hw.sendJointTorques(tau);
}

Fake 参数服务¶

ROS2 的参数系统在测试中可能引入不必要的复杂性。一个 Fake 参数提供者可以在不启动 ROS2 的情况下为控制器提供配置。

#include <string>
#include <unordered_map>

class FakeParamProvider {
public:
    void set(const std::string& key, double value) {
        params_[key] = value;
    }

    double get(const std::string& key, double default_value) const {
        auto it = params_.find(key);
        if (it != params_.end()) {
            return it->second;
        }
        return default_value;
    }

private:
    std::unordered_map<std::string, double> params_;
};

这个 Fake 不依赖 ROS2 运行时。控制器的核心算法可以在纯 C++ 单元测试中使用它，不需要启动节点、executor 或 DDS。只有在集成测试中才用真实的 ROS2 参数。

测试替身的选择决策¶

问题	推荐替身	理由
验证控制器是否调用了紧急停止	Mock	需要检查调用是否发生
提供固定传感器输入	Stub/Fake	不需要验证消费行为
替代复杂参数加载	Fake	需要简化但可工作的实现
模拟通信延迟	Fake	需要可控的延迟行为
验证力矩在安全范围内	Mock + Matcher	需要检查输出值

常见陷阱¶

类型	错误做法	现象	根本原因	正确做法
概念	Mock 过度指定内部实现	小重构导致测试全坏	测试了实现而非行为	Mock 验证外部契约
工程	所有测试都用真实 ROS2	测试慢且不稳定	依赖网络和调度	核心算法用纯 C++ 测试
编程	Fake 和真实实现行为不一致	测试通过但上机失败	Fake 过于简化	Fake 保留关键行为
思维	不用测试替身而等硬件	开发周期长	硬件不总是可用	尽早用替身测试

练习¶

为一个步态控制器写 Mock 硬件接口，验证它在急停信号后不再发送力矩。
用 Fake IMU 节点和 launch_testing 测试状态估计节点的启动行为。
设计一个 Fake 传感器，可以注入延迟和丢包，用于测试控制器的鲁棒性。

8.19 CI/CD 中的测试策略 ⭐⭐¶

这一节解决的问题是：如何在持续集成中组织测试，让反馈既快又全面。

测试分层与 CI 频率¶

不是所有测试都适合在每次提交时运行。测试应按成本和价值分层。

层级	内容	CI 频率	耗时预算	覆盖目标
L0 编译	编译所有目标	每次提交	2-5 min	语法和类型正确
L1 单元	gtest + 参数化	每次提交	1-3 min	数学和接口正确
L2 Sanitizer	ASan+UBSan	每次提交	5-10 min	内存和未定义行为
L3 集成	launch_testing	每次提交	3-5 min	节点图连接
L4 TSan	并发测试	每日	10-30 min	数据竞争
L5 回放	rosbag 回放比较	每日	10-30 min	输出回归
L6 仿真	闭环仿真场景	每日或发布前	30-60 min	闭环行为
L7 性能	benchmark 退化检测	每日	5-15 min	性能回归

性能退化检测¶

性能基准的 CI 集成需要处理一个核心问题：CI runner 的负载不稳定，单次 benchmark 结果有噪声。解决方案有两种。

第一种是相对比较。同一个 CI job 中运行基线版本和当前版本的 benchmark，比较相对变化而不是绝对值。

第二种是统计检测。连续收集多次 benchmark 结果，用移动平均和标准差检测退化。

name: performance

on:
  schedule:
    # 每日凌晨运行。
    - cron: '0 2 * * *'

jobs:
  benchmark:
    runs-on: self-hosted
    steps:
      - uses: actions/checkout@v4
      - name: Build benchmark
        run: cmake -S . -B build -DCMAKE_BUILD_TYPE=Release && cmake --build build
      - name: Run benchmark
        # 输出 JSON 格式，便于后续解析和历史比较。
        run: ./build/controller_benchmark --benchmark_format=json > results.json
      - name: Check regression
        run: python3 scripts/check_benchmark_regression.py results.json

反事实地看，如果不分层会怎样？假设所有测试（编译、单元、sanitizer、集成、仿真、回放）都在每次提交时运行，CI 总耗时可能超过一小时。开发者会开始忽略 CI 结果（因为太慢）或跳过等待直接合并（因为阻塞太久）。分层的目的不是少测，而是让快反馈和深检查各得其所。

测试隔离原则¶

CI 中的测试之间不应有隐式依赖。每个测试应该能独立运行、独立失败。

原则	做法	避免
独立构造	每个测试创建自己的对象	测试间共享全局状态
固定种子	随机测试记录并固定种子	依赖系统时间的随机
明确清理	每个测试后释放资源	文件或端口泄漏到下个测试
并行安全	测试可以并行运行	依赖固定端口号

失败处理和调试信息¶

CI 测试失败时，开发者需要足够信息来定位问题，而不需要在本地重现。

信息	价值	输出方式
失败断言的具体值	知道偏差多大	gtest `EXPECT_NEAR` 自动输出
随机种子	可以重现随机输入	测试开始时打印
输入文件路径	可以检查输入数据	测试日志
sanitizer 报告	定位内存和并发问题	stderr 输出
参数文件版本	排除配置差异	测试日志

GitHub Actions 并行测试¶

name: test

on: [push, pull_request]

jobs:
  build-and-test:
    runs-on: ubuntu-22.04
    strategy:
      matrix:
        # 并行运行不同构建配置。
        build_type: [Debug, Release]
        sanitizer: [none, asan]
    steps:
      - uses: actions/checkout@v4
      - name: Configure
        run: |
          cmake -S . -B build \
            -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \
            -DENABLE_ASAN=${{ matrix.sanitizer == 'asan' && 'ON' || 'OFF' }}
      - name: Build
        run: cmake --build build -j$(nproc)
      - name: Test
        run: ctest --test-dir build --output-on-failure -j$(nproc)

常见陷阱¶

类型	错误做法	现象	根本原因	正确做法
工程	所有测试每次提交都跑	CI 耗时一小时	重测试应降频	分层调度
编程	测试失败只输出 FAIL	无法定位问题	缺少上下文信息	输出具体值、种子和输入
概念	性能测试用绝对阈值	CI 环境变化导致误报	共享 runner 负载不稳定	相对比较或专用机器
思维	CI 绿就不用本地测试	平台差异 bug 漏网	CI 只跑一个平台	CI + 开发者本地测试

练习¶

为一个机器人项目设计 CI 配置：L0-L3 每次提交，L4-L7 每日。
写一个 benchmark 退化检测脚本：读取历史 JSON 结果，超过 p95 基线 20% 报警。
把一个测试套件改成可并行运行：消除全局状态和固定端口依赖。

本章小结¶

知识点	关键结论	工程动作
测试金字塔	越底层越便宜，越应自动化	单元、集成、回放分层
gtest/gmock	测行为契约和数学性质	fixture、参数化、mock
ROS2 回放	真实数据可重复	rosbag/mcap + 阈值比较
Sanitizer	抓内存和并发隐患	ASan/UBSan 常跑，TSan 专项
性能剖析	证据驱动优化	benchmark、perf、Tracy
数值调试	故障要能离线复现	记录最小复现包
CI	检查要自动化和分层	快检查高频，重检查定期
Fuzzing	自动发现意想不到的边界	libFuzzer + ASan 配合
Benchmark 深入	报告分位数而非只看均值	fixture 预分配 + DoNotOptimize
Mock/Fake	隔离硬件和 ROS2 依赖	核心算法纯 C++ 测试
CI 策略	分层调度降低总耗时	L0-L3 每次提交，L4+ 每日

累积项目：机器人测试与调试平台¶

本章新增模块是 robot_quality_lab。

阶段 1：为数学核心和求解器适配层添加 gtest。阶段 2：为硬件接口添加 gmock，隔离真实设备。阶段 3：为 ROS2 节点添加 launch test。阶段 4：录制一段 mcap，建立回放输出比较脚本。阶段 5：添加 ASan/UBSan、TSan、clang-tidy 和 benchmark CI。阶段 6：在控制器中加入最小复现包导出，用于离线复现数值故障。

延伸阅读¶

资料	难度	阅读目的
GoogleTest 文档	⭐	掌握测试基础
GoogleMock 文档	⭐⭐	模拟硬件接口
LLVM Sanitizer 文档	⭐⭐	定位内存和并发问题
Brendan Gregg FlameGraph 资料	⭐⭐	理解火焰图
Tracy Profiler 文档	⭐⭐	多线程实时剖析
ROS2 launch_testing 文档	⭐⭐	自动化节点图测试

故障排查手册¶

症状	可能原因	排查步骤	处理
测试偶发失败	测试共享状态或依赖时间	随机顺序重复运行	独立 fixture 和固定种子
上机才发现越界	未跑 ASan	用相同输入跑 ASan 构建	加入 CI
并发 bug 难复现	数据竞争	跑 TSan 和压力测试	原子、锁或缓冲修复
优化后反而慢	只看局部微基准	perf/Tracy 看端到端	优化真实瓶颈
回放输出不稳定	随机种子或时间源未固定	记录参数和 seed	固定输入与 clock
QP 失败无法定位	未记录约束和状态	保存最小复现包	加离线复现测试
CI 太慢	重测试全量运行	分层安排频率	快检查每次，重检查定期

阶段	频率	工具
格式检查	每次提交	clang-format
编译	每次提交	CMake/colcon
单元测试	每次提交	gtest
ASan+UBSan	每次提交或每日	sanitizer
TSan	每日或专项	sanitizer
静态分析	每次提交或每日	clang-tidy
回放测试	每日或关键分支	rosbag/mcap
性能基准	每日或发布前	benchmark

测试与调试基础设施¶

前置自测¶

本章目标¶

8.1 测试金字塔：从函数到系统 ⭐¶

动机：上机调试成本最高¶

测试层级¶

反面失败：只靠上机测试¶

练习¶

8.2 Google Test 与 gmock ⭐⭐¶

gtest 基本结构¶

Fixture¶

参数化测试¶

gmock 模拟硬件接口¶

常见陷阱¶

练习¶

8.3 ROS2 集成测试与回放测试 ⭐⭐¶

launch_testing¶

rosbag2/mcap 回放¶

PlotJuggler¶

练习¶

8.4 Sanitizer 与静态分析 ⭐⭐⭐¶

Sanitizer 对比¶

CMake 启用示例¶

ASan 能发现的典型错误¶

TSan 能发现的典型错误¶

clang-tidy 与 include-what-you-use¶

练习¶

8.5 性能剖析：benchmark、perf、Tracy ⭐⭐⭐¶

Google Benchmark¶

perf 与火焰图¶

Tracy¶

性能指标¶

练习¶

8.6 调试数值问题与控制故障 ⭐⭐⭐¶

数值故障分类¶

数值断言¶

记录最小复现数据¶

练习¶

8.7 CI 中的质量门禁 ⭐⭐¶

推荐 CI 阶段¶

失败处理原则¶

练习¶

8.8 单元测试：把数学不变量写成可执行证据 ⭐⭐⭐¶

动机：控制错误常常不是崩溃¶

不变量比硬编码结果更有价值¶

示例：SO(3) 指数映射测试¶

示例：有限差分验证 Jacobian¶

浮点阈值怎么选¶

练习¶

8.9 集成测试：验证模块契约而不是重复单元测试 ⭐⭐¶

单元测试和集成测试的分工¶

ROS2 节点图测试目标¶

示例：伪造 joint_states 并等待 command¶

集成测试的边界¶

练习¶

8.10 回放测试：把真实故障变成可重复输入 ⭐⭐⭐¶

为什么回放测试重要¶

回放测试的组成¶

从长 bag 截取关键片段¶

输出比较不要逐 bit¶

示例：回放输出比较脚本¶

回放测试分层¶

练习¶

8.11 性能测试：从平均耗时走向实时尾延迟 ⭐⭐⭐¶

平均值的陷阱¶

控制周期统计器¶

禁止热路径动态分配¶

trace 与性能测试的关系¶

练习¶

8.12 Sanitizer 深入：把 C++ 未定义行为挡在上机前 ⭐⭐⭐¶

ASan、UBSan、TSan 的互补性¶

CMake target 级 sanitizer¶

TSan 示例：双缓冲替代共享写¶

sanitizer CI 拆分¶

练习¶

8.13 仿真测试：在真实硬件之前验证闭环行为 ⭐⭐⭐¶

仿真测试的角色¶

仿真场景设计¶

仿真判据表¶

pytest 驱动仿真命令¶