文章目录
- 统计各话题Topic的发布频率
- 串行访问节点并输出
- 脚本
- 使用方法
- 结果
- 并行访问节点
- 代码
- 关键改进点
- 结果
统计各话题Topic的发布频率
由于我想统计Vins_Fusion_gpu中各个话题的统计频率,看一下平均时间,所以写了一个脚本可以自动输出所有节点的所有Topic的单次的频率。
串行访问节点并输出
脚本
#!/usr/bin/env python3
import rospy
import rosnode
import rostopic
import subprocess
import threading
import time
from collections import deque
import re
class TopicFrequencyChecker:
def __init__(self, topic, window_size=10, timeout=5):
self.topic = topic
self.window_size = window_size
self.timeout = timeout
self.timestamps = deque(maxlen=window_size)
self.lock = threading.Lock()
self.subscriber = None
self.start_time = time.time()
self.rate_info = None
def callback(self, msg):
with self.lock:
current_time = time.time()
self.timestamps.append(current_time)
def measure_rate(self):
try:
# Get the message type and topic class
topic_type, real_topic, _ = rostopic.get_topic_class(self.topic, blocking=True)
if not topic_type:
return "Topic not found or no messages published."
# Subscribe to the topic
self.subscriber = rospy.Subscriber(real_topic, topic_type, self.callback)
# Wait for messages or until timeout
start_time = time.time()
while time.time() - start_time < self.timeout:
if len(self.timestamps) >= self.window_size:
break
time.sleep(0.1)
# Calculate rate
with self.lock:
if len(self.timestamps) < 2:
self.rate_info = "No messages received within the timeout period."
else:
time_diffs = [self.timestamps[i] - self.timestamps[i-1] for i in range(1, len(self.timestamps))]
avg_rate = len(self.timestamps) / (self.timestamps[-1] - self.timestamps[0]) if len(self.timestamps) > 1 else 0
min_delta = min(time_diffs)
max_delta = max(time_diffs)
std_dev = (sum((x - (1/avg_rate))**2 for x in time_diffs) / len(time_diffs)) ** 0.5 if len(time_diffs) > 1 else 0
window = len(self.timestamps)
self.rate_info = f"average rate: {avg_rate:.3f}\n\tmin: {min_delta:.3f}s max: {max_delta:.3f}s std dev: {std_dev:.5f}s window: {window}"
# Unsubscribe from the topic
if self.subscriber:
self.subscriber.unregister()
except Exception as e:
self.rate_info = str(e)
return self.rate_info
def get_published_topics(node_name):
try:
node_info = subprocess.check_output(['rosnode', 'info', node_name], stderr=subprocess.STDOUT, text=True)
lines = node_info.splitlines()
in_publications_section = False
topics = []
for line in lines:
if "Publications:" in line:
in_publications_section = True
continue
if in_publications_section and not line.strip():
break
if in_publications_section and line.strip().startswith('*'):
topic = line.split()[1]
topics.append(topic)
return topics
except subprocess.CalledProcessError:
return []
def main():
rospy.init_node('topic_frequency_checker', anonymous=True)
nodes = rosnode.get_node_names()
for node in nodes:
print(f"Node: {node}")
topics = get_published_topics(node)
if not topics:
print(" No published topics found for this node.")
print("========================================")
continue
for topic in topics:
print(f" Topic: {topic}")
checker = TopicFrequencyChecker(topic, window_size=10, timeout=3)
result = checker.measure_rate()
print(f" {result}")
print(" ----------------------------------------")
print("========================================")
if __name__ == '__main__':
main()
- 这个 Python 脚本用于检查 ROS (Robot Operating System) 系统中各个节点发布的话题的频率。它通过订阅每个话题,收集一段时间内的消息时间戳,并计算这些消息的平均发布频率、最小间隔、最大间隔和标准差。
- __init__: 初始化类实例,设置话题名称、窗口大小(即最多记录多少个时间戳)和超时时间。
- callback: 每当收到一条消息时,记录当前的时间戳到 timestamps 队列中。
- measure_rate:
- 获取话题的消息类型和实际话题名称。
- 订阅该话题,并等待消息或直到超时。
- 收集足够的时间戳后,计算平均发布频率、最小间隔、最大间隔和标准差。
- 如果在超时时间内没有收到足够的消息,则返回提示信息。
- 最后取消订阅话题并返回频率统计结果。
- main 函数:
- 初始化一个 ROS 节点,命名为 topic_frequency_checker。
- 获取系统中所有活动节点的名称。
- 对每个节点,调用 get_published_topics 获取其发布的话题。
- 如果该节点没有发布任何话题,则打印提示信息并继续处理下一个节点。
- 对每个话题,创建一个 TopicFrequencyChecker 实例,测量该话题的频率,并打印结果。
- 每个节点处理完毕后,打印分隔符以区分不同节点的输出。
使用方法
- 新建一个python文件,并将上述内容粘贴进去:
vim cal_feq.py
- 修改权限并执行:
sudo chmod +x cal_feq.py
./cal_feq.py
- 或者也可以不修改权限直接执行:
python3 cal_feq.py
- 执行的时候确保已经运行了VINS节点。
roslaunch vins euroc.launch
rosbag play ~/catkin_ws/datasets/euroc/MH_02_easy.bag
结果
lijyhh@lijyhh-desktop:~/catkin_ws$ ./cal_frq.py
Node: /rvizvisualisation
Topic: /clicked_point
No messages received within the timeout period.
----------------------------------------
Topic: /initialpose
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/match_image/mouse_click
No messages received within the timeout period.
----------------------------------------
Topic: /move_base_simple/goal
No messages received within the timeout period.
----------------------------------------
Topic: /rosout
average rate: 25.041
min: 0.000s max: 0.103s std dev: 0.04970s window: 10
----------------------------------------
========================================
Node: /rosout
Topic: /rosout_agg
average rate: 26.270
min: 0.000s max: 0.107s std dev: 0.04804s window: 10
----------------------------------------
========================================
Node: /topic_frequency_checker_2947944_1733974050991
Topic: /rosout
average rate: 42.333
min: 0.000s max: 0.070s std dev: 0.02975s window: 10
----------------------------------------
========================================
Node: /vins_estimator
Topic: /rosout
average rate: 25.469
min: 0.000s max: 0.118s std dev: 0.04983s window: 10
----------------------------------------
Topic: /tf
average rate: 24.588
min: 0.000s max: 0.109s std dev: 0.05110s window: 10
----------------------------------------
Topic: /vins_estimator/camera_pose
average rate: 11.372
min: 0.086s max: 0.116s std dev: 0.01289s window: 10
----------------------------------------
Topic: /vins_estimator/camera_pose_right
average rate: 11.135
min: 0.085s max: 0.115s std dev: 0.01285s window: 10
----------------------------------------
Topic: /vins_estimator/camera_pose_visual
average rate: 11.110
min: 0.089s max: 0.112s std dev: 0.01230s window: 10
----------------------------------------
Topic: /vins_estimator/extrinsic
average rate: 11.079
min: 0.091s max: 0.108s std dev: 0.01166s window: 10
----------------------------------------
Topic: /vins_estimator/imu_propagate
average rate: 211.338
min: 0.000s max: 0.047s std dev: 0.01477s window: 10
----------------------------------------
Topic: /vins_estimator/key_poses
average rate: 10.983
min: 0.091s max: 0.116s std dev: 0.01254s window: 10
----------------------------------------
Topic: /vins_estimator/keyframe_point
average rate: 5.256
min: 0.110s max: 0.293s std dev: 0.05564s window: 10
----------------------------------------
Topic: /vins_estimator/keyframe_pose
average rate: 8.386
min: 0.093s max: 0.208s std dev: 0.04998s window: 10
----------------------------------------
Topic: /vins_estimator/margin_cloud
average rate: 11.069
min: 0.085s max: 0.112s std dev: 0.01209s window: 10
----------------------------------------
Topic: /vins_estimator/odometry
average rate: 10.960
min: 0.082s max: 0.119s std dev: 0.01565s window: 10
----------------------------------------
Topic: /vins_estimator/path
average rate: 11.230
min: 0.040s max: 0.135s std dev: 0.02753s window: 10
----------------------------------------
Topic: /vins_estimator/point_cloud
average rate: 10.906
min: 0.081s max: 0.125s std dev: 0.01739s window: 10
----------------------------------------
Topic: /vins_estimator/rectify_pose_left
No messages received within the timeout period.
----------------------------------------
Topic: /vins_estimator/rectify_pose_right
No messages received within the timeout period.
----------------------------------------
========================================
Node: /play_1733973836412008018
Topic: /cam0/image_raw
average rate: 22.248
min: 0.038s max: 0.063s std dev: 0.00772s window: 10
----------------------------------------
Topic: /cam1/image_raw
average rate: 22.250
min: 0.028s max: 0.064s std dev: 0.01179s window: 10
----------------------------------------
Topic: /clock
No messages received within the timeout period.
----------------------------------------
Topic: /imu0
average rate: 39309.316
min: 0.000s max: 0.000s std dev: 0.00005s window: 10
----------------------------------------
Topic: /leica/position
average rate: 18.706
min: 0.037s max: 0.090s std dev: 0.01946s window: 10
----------------------------------------
Topic: /rosout
average rate: 25.532
min: 0.000s max: 0.121s std dev: 0.05059s window: 10
----------------------------------------
========================================
Node: /loop_fusion
Topic: /loop_fusion/base_path
average rate: 8.708
min: 0.084s max: 0.329s std dev: 0.07340s window: 10
----------------------------------------
Topic: /loop_fusion/camera_pose_visual
average rate: 11.317
min: 0.072s max: 0.140s std dev: 0.02086s window: 10
----------------------------------------
Topic: /loop_fusion/margin_cloud_loop_rect
average rate: 10.790
min: 0.079s max: 0.143s std dev: 0.02340s window: 10
----------------------------------------
Topic: /loop_fusion/match_image
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/odometry_rect
average rate: 11.025
min: 0.078s max: 0.124s std dev: 0.01662s window: 10
----------------------------------------
Topic: /loop_fusion/path_1
average rate: 10.835
min: 0.082s max: 0.141s std dev: 0.02034s window: 10
----------------------------------------
Topic: /loop_fusion/path_2
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/path_3
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/path_4
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/path_5
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/path_6
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/path_7
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/path_8
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/path_9
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/point_cloud_loop_rect
average rate: 6.617
min: 0.096s max: 0.211s std dev: 0.05019s window: 10
----------------------------------------
Topic: /loop_fusion/pose_graph
average rate: 9.023
min: 0.084s max: 0.199s std dev: 0.03971s window: 10
----------------------------------------
Topic: /loop_fusion/pose_graph_path
average rate: 11.219
min: 0.084s max: 0.110s std dev: 0.01339s window: 10
----------------------------------------
Topic: /rosout
average rate: 24.987
min: 0.000s max: 0.113s std dev: 0.05054s window: 10
----------------------------------------
========================================
上述timeout是因为规定了时间,可以修改:checker = TopicFrequencyChecker(topic, window_size=10, timeout=3)
中的timeout=3
来修改时间。
并行访问节点
为了实现并行访问节点并输出结果,我们可以使用 Python 的 concurrent.futures
模块中的 ThreadPoolExecutor
。这个模块允许我们创建一个线程池,并发地执行多个任务。这样可以显著提高脚本的效率,尤其是在处理大量节点时。
代码
以下是修改后的代码,使用 ThreadPoolExecutor
来并发地检查每个节点的话题频率:
#!/usr/bin/env python3
import rospy
import rosnode
import rostopic
import subprocess
import threading
import time
from collections import deque
import re
from concurrent.futures import ThreadPoolExecutor, as_completed
class TopicFrequencyChecker:
def __init__(self, topic, window_size=10, timeout=5):
self.topic = topic
self.window_size = window_size
self.timeout = timeout
self.timestamps = deque(maxlen=window_size)
self.lock = threading.Lock()
self.subscriber = None
self.start_time = time.time()
self.rate_info = None
def callback(self, msg):
with self.lock:
current_time = time.time()
self.timestamps.append(current_time)
def measure_rate(self):
try:
# Get the message type and topic class
topic_type, real_topic, _ = rostopic.get_topic_class(self.topic, blocking=True)
if not topic_type:
return "Topic not found or no messages published."
# Subscribe to the topic
self.subscriber = rospy.Subscriber(real_topic, topic_type, self.callback)
# Wait for messages or until timeout
start_time = time.time()
while time.time() - start_time < self.timeout:
if len(self.timestamps) >= self.window_size:
break
time.sleep(0.1)
# Calculate rate
with self.lock:
if len(self.timestamps) < 2:
self.rate_info = "No messages received within the timeout period."
else:
time_diffs = [self.timestamps[i] - self.timestamps[i-1] for i in range(1, len(self.timestamps))]
avg_rate = len(self.timestamps) / (self.timestamps[-1] - self.timestamps[0]) if len(self.timestamps) > 1 else 0
min_delta = min(time_diffs)
max_delta = max(time_diffs)
std_dev = (sum((x - (1/avg_rate))**2 for x in time_diffs) / len(time_diffs)) ** 0.5 if len(time_diffs) > 1 else 0
window = len(self.timestamps)
self.rate_info = f"average rate: {avg_rate:.3f}\n\tmin: {min_delta:.3f}s max: {max_delta:.3f}s std dev: {std_dev:.5f}s window: {window}"
# Unsubscribe from the topic
if self.subscriber:
self.subscriber.unregister()
except Exception as e:
self.rate_info = str(e)
return self.rate_info
def get_published_topics(node_name):
try:
node_info = subprocess.check_output(['rosnode', 'info', node_name], stderr=subprocess.STDOUT, text=True)
lines = node_info.splitlines()
in_publications_section = False
topics = []
for line in lines:
if "Publications:" in line:
in_publications_section = True
continue
if in_publications_section and not line.strip():
break
if in_publications_section and line.strip().startswith('*'):
topic = line.split()[1]
topics.append(topic)
return topics
except subprocess.CalledProcessError:
return []
def check_node(node_name):
print(f"Node: {node_name}")
topics = get_published_topics(node_name)
if not topics:
print(" No published topics found for this node.")
print("========================================")
return
for topic in topics:
print(f" Topic: {topic}")
checker = TopicFrequencyChecker(topic, window_size=10, timeout=3)
result = checker.measure_rate()
print(f" {result}")
print(" ----------------------------------------")
print("========================================")
def main():
rospy.init_node('topic_frequency_checker', anonymous=True)
nodes = rosnode.get_node_names()
# Use ThreadPoolExecutor to run checks in parallel
with ThreadPoolExecutor(max_workers=10) as executor:
futures = [executor.submit(check_node, node) for node in nodes]
# Wait for all tasks to complete
for future in as_completed(futures):
future.result() # This will raise any exceptions that occurred during execution
if __name__ == '__main__':
main()
关键改进点
-
ThreadPoolExecutor
:- 使用
ThreadPoolExecutor
创建一个线程池,默认最大工作线程数为 10(你可以根据需要调整max_workers
参数)。 - 对每个节点调用
check_node
函数,并将其提交给线程池执行。
- 使用
-
as_completed
:- 使用
as_completed
来等待所有提交的任务完成,并按完成顺序处理结果。 future.result()
确保任何在子线程中抛出的异常都能被捕获并处理。
- 使用
-
check_node
函数:- 将原来的节点检查逻辑封装到
check_node
函数中,以便可以在多个线程中并发执行。
- 将原来的节点检查逻辑封装到
通过这些改进,脚本将并发地检查每个节点的话题频率,显著提高了处理速度。即使某些节点无法访问或响应较慢,也不会阻塞其他节点的检查。
结果
并行输出可能有些显示问题,会错行,这是正常的。
lijyhh@lijyhh-desktop:~/catkin_ws$ ./parallel_cal_frq.py
Node: /rosout
Node: /loop_fusion
Node: /vins_estimator
Node: /topic_frequency_checker_2956131_1733974476667
Node: /rvizvisualisation
Node: /play_1733974476168986985
Topic: /rosout
Topic: /rosout_agg
Topic: /clicked_point
Topic: /cam0/image_raw
Topic: /rosout
Topic: /loop_fusion/base_path
average rate: 22.355
min: 0.030s max: 0.061s std dev: 0.01066s window: 10
----------------------------------------
Topic: /cam1/image_raw
average rate: 22.568
min: 0.039s max: 0.061s std dev: 0.01054s window: 10
----------------------------------------
Topic: /clock
average rate: 19.122
min: 0.000s max: 0.286s std dev: 0.08556s window: 10
----------------------------------------
Topic: /vins_estimator/camera_pose
average rate: 19.123
min: 0.000s max: 0.286s std dev: 0.08559s window: 10
----------------------------------------
========================================
average rate: 41.538
min: 0.000s max: 0.061s std dev: 0.02991s window: 10
----------------------------------------
========================================
average rate: 19.031
min: 0.050s max: 0.065s std dev: 0.00737s window: 10
----------------------------------------
Topic: /vins_estimator/camera_pose_right
average rate: 17.956
min: 0.045s max: 0.103s std dev: 0.01937s window: 10
----------------------------------------
Topic: /loop_fusion/camera_pose_visual
average rate: 18.924
min: 0.051s max: 0.066s std dev: 0.00694s window: 10
----------------------------------------
Topic: /vins_estimator/camera_pose_visual
average rate: 19.454
min: 0.048s max: 0.064s std dev: 0.00702s window: 10
----------------------------------------
Topic: /loop_fusion/margin_cloud_loop_rect
No messages received within the timeout period.
----------------------------------------
Topic: /initialpose
average rate: 19.143
min: 0.048s max: 0.067s std dev: 0.00775s window: 10
----------------------------------------
Topic: /vins_estimator/extrinsic
average rate: 18.730
min: 0.048s max: 0.064s std dev: 0.00783s window: 10
----------------------------------------
Topic: /loop_fusion/match_image
average rate: 19.981
min: 0.050s max: 0.064s std dev: 0.00703s window: 10
----------------------------------------
Topic: /vins_estimator/imu_propagate
average rate: 1023000.976
min: 0.000s max: 0.000s std dev: 0.00000s window: 10
----------------------------------------
Topic: /vins_estimator/key_poses
No messages received within the timeout period.
----------------------------------------
Topic: /imu0
average rate: 26698.307
min: 0.000s max: 0.000s std dev: 0.00007s window: 10
----------------------------------------
Topic: /leica/position
average rate: 17.805
min: 0.052s max: 0.072s std dev: 0.00865s window: 10
----------------------------------------
Topic: /vins_estimator/keyframe_point
average rate: 16.714
min: 0.056s max: 0.117s std dev: 0.01910s window: 10
----------------------------------------
Topic: /vins_estimator/keyframe_pose
average rate: 21.437
min: 0.039s max: 0.088s std dev: 0.01580s window: 10
----------------------------------------
Topic: /rosout
average rate: 23.558
min: 0.000s max: 0.109s std dev: 0.05290s window: 10
----------------------------------------
========================================
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/match_image/mouse_click
average rate: 9.791
min: 0.059s max: 0.193s std dev: 0.04560s window: 10
----------------------------------------
Topic: /vins_estimator/margin_cloud
average rate: 10.856
min: 0.110s max: 0.142s std dev: 0.03367s window: 4
----------------------------------------
Topic: /loop_fusion/odometry_rect
average rate: 11.175
min: 0.086s max: 0.111s std dev: 0.01229s window: 10
----------------------------------------
Topic: /vins_estimator/odometry
average rate: 11.100
min: 0.087s max: 0.111s std dev: 0.01251s window: 10
----------------------------------------
Topic: /loop_fusion/path_1
average rate: 10.991
min: 0.094s max: 0.112s std dev: 0.01194s window: 10
----------------------------------------
Topic: /vins_estimator/path
average rate: 11.347
min: 0.045s max: 0.203s std dev: 0.04640s window: 10
----------------------------------------
Topic: /loop_fusion/path_2
No messages received within the timeout period.
----------------------------------------
Topic: /move_base_simple/goal
average rate: 11.246
min: 0.087s max: 0.120s std dev: 0.01348s window: 10
----------------------------------------
Topic: /vins_estimator/point_cloud
average rate: 11.160
min: 0.081s max: 0.112s std dev: 0.01356s window: 10
----------------------------------------
Topic: /vins_estimator/rectify_pose_left
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/path_3
No messages received within the timeout period.
----------------------------------------
Topic: /rosout
average rate: 24.035
min: 0.000s max: 0.113s std dev: 0.05193s window: 10
----------------------------------------
========================================
No messages received within the timeout period.
----------------------------------------
Topic: /vins_estimator/rectify_pose_right
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/path_4
No messages received within the timeout period.
----------------------------------------
========================================
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/path_5
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/path_6
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/path_7
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/path_8
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/path_9
No messages received within the timeout period.
----------------------------------------
Topic: /loop_fusion/point_cloud_loop_rect
average rate: 22.517
min: 0.089s max: 0.089s std dev: 0.00000s window: 2
----------------------------------------
Topic: /loop_fusion/pose_graph
average rate: 10.780
min: 0.095s max: 0.117s std dev: 0.01202s window: 10
----------------------------------------
Topic: /loop_fusion/pose_graph_path
average rate: 10.005
min: 0.058s max: 0.136s std dev: 0.02569s window: 10
----------------------------------------
Topic: /rosout
average rate: 24.986
min: 0.000s max: 0.114s std dev: 0.05046s window: 10
----------------------------------------
========================================