ros2_topic_hz_命令实现_实时统计频率不准问题
1.问题与需求
问题: ros2 topic hz 命令实时统计频率不准确
验证步骤:
- 实现一个动态频率发布topic 的节点node
- 发布频率从10Hz, 切换到 100Hz
- ros2 topic hz 统计话题频率变化
5_pub_动态频率发布.py
#!/usr/bin/env python3
# coding:utf-8
import time
import threadingimport rclpy
from rclpy.node import Nodefrom std_msgs.msg import String, Float64, Float64MultiArray, Int64, UInt64MultiArray
from nobloack_key_input import Noblock_terminalclass MinimalPublisher(Node):def __init__(self):super().__init__('minimal_publisher')self.pub_str = self.create_publisher(String, '/hz_string', 1)self.pub_int = self.create_publisher(Int64, '/hz_int', 1)self.pub_int_array = self.create_publisher(UInt64MultiArray, '/hz_int_array', 1)self.pub_float = self.create_publisher(Float64, '/hz_float', 1)self.pub_float_array = self.create_publisher(Float64MultiArray, '/hz_float_array', 1)self.delay_time = 0.1self.thread_init()self.dynamic_rate_pub()def thread_init(self):t1 = threading.Thread(target=self.change_delay_time, daemon=True)t1.start()def dynamic_rate_pub(self):data_str = 'hello world'data_int_array = [1, 2, 3, 4, 5]data_float_array = [1.0, 2.0, 3.0, 4.0, 5.0]while True:# self.get_logger().info('Publishing: "%s"' % msg.data)self.pub_str.publish(String(data=data_str))self.pub_int.publish(Int64(data=1))self.pub_int_array.publish(UInt64MultiArray(data=data_int_array))self.pub_float.publish(Float64(data=1.0))self.pub_float_array.publish(Float64MultiArray(data=data_float_array))time.sleep(self.delay_time)def change_delay_time(self):help_str="""
\033[80D w +0.01s
\033[80D s -0.01s
\033[80D q 退出
\033[80D
"""print(help_str)noblock_term = Noblock_terminal()time_stamp = time.time()while True:key = noblock_term.select_cmd(timeout=0.5)if key == 'w':self.delay_time += 0.01elif key == 's':self.delay_time -= 0.01if self.delay_time <= 0:self.delay_time = 0.01elif key == 'q':breakelif key == '0':continueelse:print("\033[80D 未知指令")print(f"\033[80D delay_time: {self.delay_time:.2f}")# 定时打印 使用帮助cur_time = time.time()if int(cur_time - time_stamp) % 10 == 0:print(help_str)print("退出 控制")noblock_term.stop_no_block()def main(args=None):rclpy.init(args=args)minimal_publisher = MinimalPublisher()rclpy.spin(minimal_publisher)minimal_publisher.destroy_node()rclpy.shutdown()if __name__ == '__main__':main()
完整代码链接: gitee在线代码
- nobloack_key_input.py
- 5_pub_动态频率发布.py
代码解析:
- 发布多个话题 /hz_int, /hz_string, /hz_float
- 变量 self.delay_time, 是发布间隔时间
- 通过键盘命令 w增加/ s减少 self.delay_time的值, 从而修改发布间隔时间
如图:
当delay_time 从 0.1s -->变为–> 0.01s
频率 对应从 10hz -->变为–> 100hz
实际情况却是, ros2 topic hz 统计频率为 10, 15, 27,36,43…99.8hz
即ros2 topic hz 统计频率变化时, 是有一套统计中值方案
需求
实现一个能够实时反应当前真实频率的统计方式
2.代码实现
1.参考ros2 topic hz命令源代码
linux系统安装ros2 humble之后, 本地代码: /opt/ros/humble/lib/python3.10/site-packages/ros2topic/verb/hz.py
在线代码: https://github.com/ros2/ros2cli/blob/rolling/ros2topic/ros2topic/verb/hz.py
def get_hz(self, topic=None):if not self.get_times(topic=topic):returnelif self.get_msg_tn(topic=topic) == self.get_last_printed_tn(topic=topic):returnwith self.lock:# Get frequency every one secondtimes = self.get_times(topic=topic)n = len(times)mean = sum(times) / nrate = 1. / mean if mean > 0. else 0# std devstd_dev = math.sqrt(sum((x - mean)**2 for x in times) / n)# min and maxmax_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
源码解析:
- 每隔1s, 执行
get_hz()- times 是列表, 统计1s内, 每帧时间间隔
- 获取统计值 帧率n, 平均间隔mean, 最大延迟 max, 最小延迟min
- 计算 平均帧率rate, 标准差延迟std_dev
具体 为什么ros2 topic hz统计是递增/递减, 可以看代码统计方法实现
2.实时更新统计版本
3_subscribe_hz_订阅统计_模板版.py
@dataclass
class Sub_Topic_Hz:name: str # 话题名value_name: str # 去除特殊符号, 话题名 --> 用作python 动态变量,动态函数名 创建fixed_hz: int # 固有频率fixed_hz_diff_time: float # 固定帧时间差msg_type: any = None# 记录时间, 数据hz: float = 0.0min_time: float = 1000.0max_time: float = 0.0new_start_time: float = 0.0 # 上一秒开始统计时间last_time: float = 0.0 # 上一条topic话题时间times: List[float] = field(default_factory=list)# 统计时间accum_dev_time: float = 0.0 # 积累偏差std_dev_time: float = 0.0 # 标准偏差Config = {"record_path": "/tmp","csv_record": True,"csv_filed": ["time","topic","rate","min","max","accum_dev","std_dev",],
}sub_topic_list = [Sub_Topic_Hz("/hz_string", "hz_string", 100, 1.0 / 100, msg_type=String),Sub_Topic_Hz("/hz_int", "hz_int", 100, 1.0 / 100, msg_type=Int64),Sub_Topic_Hz("/hz_int_array", "hz_int_array", 50, 1.0 / 50, msg_type=UInt64MultiArray),Sub_Topic_Hz("/hz_float", "hz_float", 100, 1.0 / 100, msg_type=Float64),Sub_Topic_Hz("/hz_float_array", "hz_float_array", 50, 1.0 / 50, msg_type=Float64MultiArray),
]# 完整代码, 看gitee
# ...
# 完整代码, 看giteedef callback_hz_string(self, msg):hz = self.hz_stringself.cal_hz(hz)def callback_hz_float(self, msg):hz = self.hz_floatself.cal_hz(hz)
代码解析:
参考ros2 hz.py 代码实现,
- 统计每秒内 rate, min, max, std_dev
- 新增统计 accum_dev_time: float = 0.0 # 积累偏差
解释: 假如一个topic 预订发布频率 10hz
第1帧预订接受时间: 0.10s
第2帧预订接受时间: 0.20s
第3帧预订接受时间: 0.30s
…
第10帧预订接受时间: 1.00s
accum_dev_time计算过程:
将每一帧的接受时间, 与预订接受时间, 作差取绝对值, 统计累计偏差之和
即统计系统调度的精准性- 新增话题统计, 按模板添加到
sub_topic_list, 并实现一个回调函数,如callback_xxxx
3.测试验证
终端1: 运行动态频率发布脚本
./5_pub_动态频率发布.py
终端2: ros2 topic hz统计
ros2 topic hz /hz_int
终端3: 运行测试程序
./3_subscribe_hz_订阅统计_模板版.py
测试1: 发布频率10hz
终端1:
默认0.1s, 即10hz
./5_pub_动态频率发布.pyw +0.01s
s -0.01s
q 退出
终端2
ros2 topic hz /hz_int
average rate: 9.926
min: 0.100s max: 0.101s std dev: 0.00014s window: 11
average rate: 9.926
min: 0.100s max: 0.101s std dev: 0.00011s window: 21
终端3
./3_subscribe_hz_订阅统计_模板版.py
/hz_string hz:9.0 rate:9.92 min:0.101 max:0.101 accum_dev:5.086 std dev:0.000
/hz_int hz:9.0 rate:9.92 min:0.101 max:0.101 accum_dev:5.086 std dev:0.000
/hz_int_array hz:9.0 rate:9.94 min:0.099 max:0.101 accum_dev:4.617 std dev:0.001
/hz_float hz:9.0 rate:9.94 min:0.099 max:0.101 accum_dev:5.069 std dev:0.001
测试结论:
3_subscribe_hz_订阅统计_模板版.py 统计结果正确
测试2: 发布频率100hz
终端1
按 s 减少delay_time 到 0.01
./5_pub_动态频率发布.pyw +0.01s
s -0.01s
q 退出delay_time: 0.09
delay_time: 0.08
delay_time: 0.07
delay_time: 0.06
delay_time: 0.05
delay_time: 0.04
delay_time: 0.03
delay_time: 0.02
delay_time: 0.01
终端2
ros2 topic hz /hz_int
average rate: 12.608
min: 0.020s max: 0.101s std dev: 0.03208s window: 103
average rate: 20.830
min: 0.010s max: 0.101s std dev: 0.04129s window: 191
average rate: 28.096
min: 0.010s max: 0.101s std dev: 0.03806s window: 286
average rate: 34.061
min: 0.010s max: 0.101s std dev: 0.03470s window: 381
average rate: 39.041
min: 0.010s max: 0.101s std dev: 0.03194s window: 476
average rate: 43.257
min: 0.010s max: 0.101s std dev: 0.02969s window: 571
终端3
./3_subscribe_hz_订阅统计_模板版.py
/hz_int_array hz:63 rate:63.42 min:0.010 max:0.031 accum_dev:6.675 std dev:0.006
/hz_float hz:63 rate:63.43 min:0.010 max:0.031 accum_dev:18.912 std dev:0.006
/hz_float_array hz:63 rate:63.44 min:0.010 max:0.031 accum_dev:6.674 std dev:0.006
/hz_string hz:93 rate:93.82 min:0.010 max:0.011 accum_dev:3.909 std dev:0.000
/hz_int hz:93 rate:93.85 min:0.010 max:0.011 accum_dev:3.885 std dev:0.000
/hz_int_array hz:93 rate:93.86 min:0.009 max:0.011 accum_dev:41.827 std dev:0.000
/hz_float hz:93 rate:93.88 min:0.009 max:0.011 accum_dev:3.843 std dev:0.000
/hz_float_array hz:93 rate:93.89 min:0.009 max:0.012 accum_dev:41.870 std dev:0.000
/hz_string hz:94 rate:94.29 min:0.010 max:0.011 accum_dev:3.749 std dev:0.000
/hz_int hz:94 rate:94.31 min:0.010 max:0.011 accum_dev:3.720 std dev:0.000
结果如图
4.总结
- 后续尝试向ros2 github 提交PR, 添加:
- feat: 新增统计实时真实帧率选项
- feat: 新增统计结果保存为csv格式(或sql,其它格式), 用作性能统计分析,绘图
- 当别人问你为什么 ros2 topic hz 有一个缓慢的波动是, 可以解释:
ros2 topic hz代码设计如此, 统计上是曲线变动, 实际上已经变化到另一个值 - 如图在nvidia Jetson Orix – ubuntu20.04上, 统计部分话题, 默认50Hz频率, 在高负载下, 使用我的统计程序, 频率波动很大, 故代码 和 系统性能 都需要优化
