编写 Python 代码，实现 ROS2 中 ros2 topic hz 命令的功能：实时统计并输出指定话题的消息发布频率

[opp36000]

其实 ros2 topic hz /topic这个命令本身的实现，也是基于python的。这个源码可以/opt/ros/jazzy/lib/python3.12/site-packages/ros2topic/verb/hz.py路径进行查看；也可以参考github上的源码库：https://github.com/suhyeongyu/topic_hz_profile

一、通过ros2 时钟方法进行计算频率原理

ROS 2 的 rclpy 库中对 Clock（时钟）模块进行获取时间。下面我们先看官方的源码，然后再根据源码进行实战，写一个python脚本，对话题频率机型统计。

首先要导入的模块有：
● import rclpypython 版本的rclpy库，可以用来创建节点、话题、服务、定时器。是我们做ros2时用到的库。
● from rclpy.clock import Clockros2用节点时钟，可以获得时间。这个对后面计算频率会用到。

其次，我们看用来计算频率的 ROSTopicHz类。这是ros2官方计算频率的类。我们一一看一整体。

ros官方ros2 topic hz源码

class ROSTopicHz(object):
"""ROSTopicHz receives messages for a topic and computes frequency."""

def __init__(self, node, window_size, filter_expr=None, use_wtime=False):
    self.lock = threading.Lock()
    self.last_printed_tn = 0
    self.msg_t0 = -1   ## 初始时间msg_t0 < 0
    self.msg_tn = 0    ##上一条消息时间
    self.times = []    ##间隔列表
    self._last_printed_tn = defaultdict(int)
    self._msg_t0 = defaultdict(lambda: -1)  ## 兼容新方法，msg_t0 < 0表示未初始化
    self._msg_tn = defaultdict(int)## 新方法为字典dict，可以支持写入多个topic
    self._times = defaultdict(list)
    self.filter_expr = filter_expr
    self.use_wtime = use_wtime  ##是否用系统的时间

    self.window_size = window_size  ## 滑动窗口的大小

    # Clock that has support for ROS time.（即通过ros节点获取时间）
    self._clock = node.get_clock()

def get_last_printed_tn(self, topic=None):
    ## 获取最新的打印时间
    if topic is None:
        return self.last_printed_tn
    return self._last_printed_tn[topic]  ##新方法为字典dict，可以支持写入多个topic

def set_last_printed_tn(self, value, topic=None):
    ## 设置最新的打印时间
    if topic is None:
        self.last_printed_tn = value
    self._last_printed_tn[topic] = value

def get_msg_t0(self, topic=None):
    ##获取初始时间
    if topic is None:
        return self.msg_t0
    return self._msg_t0[topic]

def set_msg_t0(self, value, topic=None):
    ##设置为初始时间
    if topic is None:
        self.msg_t0 = value
    self._msg_t0[topic] = value

def get_msg_tn(self, topic=None):
    ##获取上一条消息时间
    if topic is None:
        return self.msg_tn
    return self._msg_tn[topic]

def set_msg_tn(self, value, topic=None):
    ###设置为上一条消息时间
    if topic is None:
        self.msg_tn = value
    self._msg_tn[topic] = value

def get_times(self, topic=None):
    ##获取间隔列表
    if topic is None:
        return self.times
    return self._times[topic]

def set_times(self, value, topic=None):
    ## 设置间隔列表
    if topic is None:
        self.times = value
    self._times[topic] = value

def callback_hz(self, m, topic=None):
    """
    Calculate interval time.

    :param m: Message instance
    :param topic: Topic name
    """
    # （即过滤掉不匹配的消息）
    if self.filter_expr is not None and not self.filter_expr(m):
        return
    with self.lock:

        ## 如果self.use_wtime为True就使用系统时间，否则就用node.get_clock()的时间
        curr_rostime = self._clock.now() if not self.use_wtime else \
        Clock(clock_type=ClockType.SYSTEM_TIME).now()

        # 如果当前curr_rostime时间的nanoseconds参数为0，表示 ROS 时间尚未激活
        if curr_rostime.nanoseconds == 0:
            if len(self.get_times(topic=topic)) > 0:##间隔列表（self._times = defaultdict(list)）
                ## 重置一下时间并退出，等ros激活后再试
                print('time has reset, resetting counters')
                self.set_times([], topic=topic)  ##清空间隔列表
            return

        curr = curr_rostime.nanoseconds
        msg_t0 = self.get_msg_t0(topic=topic) ## 获取一下初始时间
        if msg_t0 < 0 or msg_t0 > curr: 
            ## 如果 msg_t0 < 0（还未初始化）
            ##或者 msg_t0 > curr（新消息时间比第一条还早？异常！）
            ##则重置该话题时间
            self.set_msg_t0(curr, topic=topic)
            self.set_msg_tn(curr, topic=topic)
            self.set_times([], topic=topic)  ##清空间隔列表
        else:
            ##添加间隔到间隔列表，间隔计算: 当前时间（curr）- 上一条消息时间
            self.get_times(topic=topic).append(curr - self.get_msg_tn(topic=topic))
            self.set_msg_tn(curr, topic=topic)
        ##如果间隔列表大于滑动窗口，就移除索引为 0 的元素，即list[0]最老的一个消息。
        if len(self.get_times(topic=topic)) > self.window_size:
            self.get_times(topic=topic).pop(0)

def get_hz(self, topic=None):
    """
    Calculate the average publising rate.

    :param topic: topic name, ``list`` of ``str``
    :returns: tuple of stat results
        (rate, min_delta, max_delta, standard deviation, window number)
        None when waiting for the first message or there is no new one
    """
    #没有间隔数据，返回
    if not self.get_times(topic=topic):
        return
    ##如果 last_printed_tn == 0（还未输出过）
    elif self.get_last_printed_tn(topic=topic) == 0:
        self.set_last_printed_tn(self.get_msg_tn(topic=topic), topic=topic)
        return
    #时间 msg_tn 比上次输出时间晚不到 1 秒（1e9 纳秒），则返回 None（时间未到，不计算）。
    elif self.get_msg_tn(topic=topic) < self.get_last_printed_tn(topic=topic) + 1e9:
        return
    with self.lock:
        # Get frequency every one minute
        times = self.get_times(topic=topic)
        n = len(times)
        ## 计算平均间隔
        mean = sum(times) / n
        rate = 1. / mean if mean > 0. else 0

        # std dev
        std_dev = math.sqrt(sum((x - mean)**2 for x in times) / n)

        ##计算最大间隔和最小间隔
        max_delta = max(times)
        min_delta = min(times)

        self.set_last_printed_tn(self.get_msg_tn(topic=topic), topic=topic)

    return rate, min_delta, max_delta, std_dev, n

def print_hz(self, topic=None):
    """Print the average publishing rate to screen."""
    ## 用来打印的函数
    ret = self.get_hz(topic)
    if ret is None:
        return
    rate, min_delta, max_delta, std_dev, window = ret
    print('average rate: %.3f\n\tmin: %.3fs max: %.3fs std dev: %.5fs window: %s'
          % (rate * 1e9, min_delta * 1e-9, max_delta * 1e-9, std_dev * 1e-9, window))
    return

● 看一下这个init函数：
def init(self, node, window_size, filter_expr=None, use_wtime=False):
● node：ROS 2 节点对象，用于获取时钟（node.get_clock()），时钟可以是 ROS 时间或系统时间。
● window_size：滑动窗口大小，即保留最近多少条消息的间隔时间用于计算。
● filter_expr：可选的可调用对象，用于过滤消息（例如只统计满足某些条件的消息）。
● use_wtime：如果为 True，使用系统墙壁时间；否则使用节点时钟（ROS 时间，如果开启则使用 /clock 话题时间，否则是系统时间）。
def init(self, node, window_size, filter_expr=None, use_wtime=False):
self.lock = threading.Lock()
self.last_printed_tn = 0
self.msg_t0 = -1
self.msg_tn = 0
self.times = []
self._last_printed_tn = defaultdict(int)
self._msg_t0 = defaultdict(lambda: -1)
self._msg_tn = defaultdict(int)
self._times = defaultdict(list)
self.filter_expr = filter_expr
self.use_wtime = use_wtime

    self.window_size = window_size

    # Clock that has support for ROS time.
    self._clock = node.get_clock()

● 为了支持同时处理多个话题，它使用了两个层面的存储：
● 旧版字段（self.times, self.msg_t0, self.msg_tn, self.last_printed_tn）——这些用于单个话题（历史遗留）。
● 新版字典（self._times, self._msg_t0, self._msg_tn, self._last_printed_tn）——这些是 defaultdict，以 topic 名称为键存储不同话题的数据。
但为了方便，所有公开的 get_xxx(topic=None) 和 set_xxx 方法都做了兼容：如果 topic 为 None，就操作旧字段；否则操作对应的字典。

实战测试;

import rclpy
from rclpy.node import Node
from ros2topic.api import get_msg_class
from sensor_msgs.msg import Imu
from collections import defaultdict
from rclpy.clock import Clock
from rclpy.clock import ClockType
import time
import threading
import math

## 要求的频率
DATA_TOPIC_HZ = 195
TOPIC_NAME = '/imu_data'


class ROSTopicHz(object):
    """ROSTopicHz receives messages for a topic and computes frequency."""
    pass
    ## 这里不解释了，把上面的方法粘贴进来即可

 

class TopicHzMonitor(Node):
    def __init__(self, topic_name, window_size=100, use_wall_time=False):
        super().__init__('topic_hz_monitor')
        self.topic = topic_name
        self.window_size = window_size
        self.use_wall_time = use_wall_time

        # 等待话题出现并获取消息类型
        msg_type = get_msg_class(self, self.topic, blocking=True)
        if msg_type is None:
            raise RuntimeError(f"Topic {self.topic} not available")

        # 创建统计器（复用上方的ROSTopicHz）
        self.stats = ROSTopicHz(self, window_size, use_wtime=use_wall_time)

        # 订阅话题（QoS 可自定义）
        self.sub = self.create_subscription(
            Imu,
            self.topic,
            lambda msg: self.stats.callback_hz(msg, topic=self.topic),
            10  # 或使用 qos_profile_sensor_data
        )

    def get_current_frequency(self):
        """返回当前平均频率 (Hz) 及统计信息，如果没有数据则返回 None"""
        result = self.stats.get_hz(self.topic)
        if result is None:
            return None
        rate, min_delta, max_delta, std_dev, window = result
        # rate 单位是 1/纳秒，需要换算为 Hz
        return {
            'avg_hz': rate * 1e9,
            'min_interval_s': min_delta * 1e-9,
            'max_interval_s': max_delta * 1e-9,
            'std_dev_s': std_dev * 1e-9,
            'sample_count': window
        }



class TestHandControl:
    # pytest测试类
    @classmethod
    def setup_class(cls):
        ## 初始化rclpy库
        rclpy.init()
        cls.monitor = TopicHzMonitor('/imu_data', window_size=100)


    @classmethod
    def teardown_class(cls):
        ## 清理ros2节点和rclpy库
        cls.monitor.destroy_node()
        rclpy.shutdown()

    # 测试方法
    def test_motor_calibration(self):
        print("        * 执行测试用例: 头部IMU频率统计")
        start_time = time.monotonic()
        freq_readings = []  # 收集所有 avg_hz 采样值
        while rclpy.ok() and (time.monotonic() - start_time) < 10:
            rclpy.spin_once(self.monitor, timeout_sec=0.1)
            freq_data = self.monitor.get_current_frequency()
            if freq_data:
                print(f"[{time.time()}] Avg Hz: {freq_data['avg_hz']:.2f}, min interval: {freq_data['min_interval_s']:.4f}s")
                freq_readings.a
ppend(freq_data['avg_hz'])

        # 断言 1：确保有数据被采集到
        assert len(freq_readings) > 0, "10s内未收集到任何IMU频率数据"

        # 断言 2：低于 195Hz 的采样占比必须 < 80%
        low_freq_count = sum(1 for hz in freq_readings if hz < DATA_TOPIC_HZ)
        low_freq_ratio = low_freq_count / len(freq_readings)
        assert low_freq_ratio < 0.8, \
            f"频率不合格: {low_freq_ratio*100:.1f}% 的采样值低于195Hz ({low_freq_count}/{len(freq_readings)})"