4、编写延迟测试节点：自定义话题发布与订阅，记录时间戳

好，咱们进入实战环节。

前面几章我们聊了延迟的成因和理论模型。说实话，理论再漂亮，不如自己动手测一把来得实在。我当年刚接触ROS2时，就被一个诡异的延迟问题折磨了两天。后来发现，其实就是测试节点的时间戳记录方式不对。

这一节，我们就来亲手写一个延迟测试节点。核心思路很简单：发布端打上时间戳，订阅端收到后算差值。但这里面的坑，可不少。

4.1 为什么需要自定义话题？

你可能会问：ROS2自带的话题类型那么多，直接用不行吗？

嗯，这里要注意。标准话题类型（比如std_msgs/msg/String）虽然能用，但缺少一个关键字段——时间戳。虽然ROS2的消息头里自带stamp字段，但很多开发者并不用它来记录发布时刻，而是记录采集时刻。这两者是有区别的。

我个人习惯的做法是：自定义一个专门用于延迟测试的消息类型。里面只放两个东西：一个序列号，一个时间戳。干净利落，没有干扰。

4.2 定义自定义消息

首先，在包目录下创建msg文件夹，新建一个DelayTest.msg文件：

# DelayTest.msg
# 用于测试端到端延迟的消息

uint32 seq          # 序列号，用于追踪丢包
builtin_interfaces/Time timestamp  # 发布时刻的时间戳

然后在CMakeLists.txt里注册这个消息类型：

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/DelayTest.msg"
  DEPENDENCIES builtin_interfaces
)

别忘了在package.xml里添加依赖：

<build_depend>builtin_interfaces</build_depend>
<exec_depend>builtin_interfaces</exec_depend>

4.3 编写发布端节点

发布端的工作很简单：每次发布前，记录当前时间。但这里有个细节——用now()还是用get_clock()->now()？

我建议用后者。为什么？因为get_clock()->now()会考虑ROS2的时间系统（比如模拟时间），而now()直接取系统时间。在仿真测试中，这个区别很重要。

// delay_publisher.cpp
#include "rclcpp/rclcpp.hpp"
#include "your_package/msg/delay_test.hpp"

using namespace std::chrono_literals;

class DelayPublisher : public rclcpp::Node
{
public:
  DelayPublisher() : Node("delay_publisher"), seq_(0)
  {
    publisher_ = this->create_publisher<your_package::msg::DelayTest>("delay_topic", 10);
    timer_ = this->create_wall_timer(100ms, std::bind(&DelayPublisher::timer_callback, this));
  }

private:
  void timer_callback()
  {
    auto msg = your_package::msg::DelayTest();
    msg.seq = seq_++;
    msg.timestamp = this->get_clock()->now();  // 关键：记录发布时刻

    publisher_->publish(msg);
    RCLCPP_INFO(this->get_logger(), "Published seq: %d", msg.seq);
  }

  rclcpp::Publisher<your_package::msg::DelayTest>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
  uint32_t seq_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<DelayPublisher>());
  rclcpp::shutdown();
  return 0;
}

4.4 编写订阅端节点

订阅端收到消息后，立刻记录当前时间，然后计算差值。这个差值就是端到端延迟。

// delay_subscriber.cpp
#include "rclcpp/rclcpp.hpp"
#include "your_package/msg/delay_test.hpp"

class DelaySubscriber : public rclcpp::Node
{
public:
  DelaySubscriber() : Node("delay_subscriber")
  {
    subscription_ = this->create_subscription<your_package::msg::DelayTest>(
      "delay_topic", 10, std::bind(&DelaySubscriber::topic_callback, this, std::placeholders::_1));
  }

private:
  void topic_callback(const your_package::msg::DelayTest::SharedPtr msg)
  {
    auto now = this->get_clock()->now();
    rclcpp::Duration latency = now - msg->timestamp;

    RCLCPP_INFO(this->get_logger(), 
      "Received seq: %d, latency: %.3f ms",
      msg->seq, latency.seconds() * 1000.0);
  }

  rclcpp::Subscription<your_package::msg::DelayTest>::SharedPtr subscription_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<DelaySubscriber>());
  rclcpp::shutdown();
  return 0;
}

4.5 时间戳对齐：一个容易被忽略的问题

你以为这样就完了？不，还有个坑。

发布端和订阅端如果不在同一台机器上，它们的系统时间可能不一致。你算出来的延迟，其实是时间差 + 真实延迟。如果两台机器的时间差是100ms，那你的测试结果就完全没意义了。

我曾经在分布式系统上做测试，结果延迟数据忽大忽小。排查了半天，才发现是NTP服务没同步好。

4.6 运行测试

编译后，分别启动两个节点：

# 终端1
ros2 run your_package delay_publisher

# 终端2
ros2 run your_package delay_subscriber

你会看到类似这样的输出：

[INFO] [delay_subscriber]: Received seq: 1, latency: 0.523 ms
[INFO] [delay_subscriber]: Received seq: 2, latency: 0.498 ms
[INFO] [delay_subscriber]: Received seq: 3, latency: 0.511 ms

这个延迟值包含了：序列化时间 + 传输时间 + 反序列化时间 + 回调执行时间。如果你发现延迟突然变大，那就要排查了——是不是回调里做了耗时操作？是不是网络拥堵？

4.7 知识体系总览

下面这张图，帮你理清整个测试节点的逻辑：

4.8 小结

这一节我们亲手搭建了一个延迟测试节点。说白了，就是发布端打时间戳，订阅端算差值。但别小看这个简单逻辑——它是我排查ROS2延迟问题的第一把刀。

你想想看，有了这个工具，你就能量化你的系统延迟。是1ms还是10ms？是稳定还是抖动？数据说话，比猜靠谱多了。

嗯，下一节我们会在这个基础上，加入统计功能，看看延迟的分布情况。不过那是后话了。

可可精品资料库