文章目录
- 统计各话题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
  ----------------------------------------
========================================



















