使用 Jupyter 分析 ROS 消息时间间隔抖动数据

ROS 是一个分布式机器人操作系统软件框架,节点之间通过松耦合的方式进行组合,包括使用 Topic、RPC 服务和参数服务器等方式进行通信。其中,Topic 是最常见的一种通信方式,例如一个雷达传感器节点实时采集三维点云数据,通过 Topic 发布到 ROS 系统,而 ROS 系统中的其他节点(如 Rviz)可以订阅这个 Topic,接收来自雷达的点云数据,将其显示出来。

案例背景

在使用 Rviz 显示雷达点云数据的过程中,我们发现 Rviz2 的三维视窗周期性地出现卡顿。数据由雷达驱动节点发布,Rviz2 节点订阅,我们想要知道卡顿的具体情况,进一步分析卡顿是由于发布端慢了还是订阅端慢了导致的,因此需要对数据进行分析。

每一个 ROS 消息都带有时间戳信息(字段名为 stamp),在 ROS2 中,可以通过 ros2 topic echo 命令打印指定 Topic 的信息。假设雷达的 Topic 为 /lidar_topic,通过下面命令即可过滤出每一条消息的时间戳信息。

ros2 topic echo /lidar_topic | grep -w -A 2 "stamp"

打印内容如下所示,ROS 消息的时间戳包括秒和纳秒两部分。

  stamp:
    sec: 1702388539
    nanosec: 988327026
--
  stamp:
    sec: 1702388540
    nanosec: 107672930
--
  stamp:
    sec: 1702388540
    nanosec: 226908922
--
  stamp:
    sec: 1702388540
    nanosec: 346390963
--

为了采集足够多的数据,我们将输出的内容写入到文件中,以便后续处理。

ros2 topic echo /lidar_topic | grep -w -A 2 "stamp" > ts-data.txt

作为对比,我们关闭 Rviz2,只启动雷达驱动节点,再采集一组数据。

ros2 topic echo /lidar_topic | grep -w -A 2 "stamp" > ts-data-quiet.txt

分析数据

新建一个终端,执行 jupyter-notebook 命令,打开 Jupyter Notebook 浏览器界面。然后点击 “新建”,选择 Python3,开始编写 Python 代码。

程序的流程大致如下:

  1. 读取文件内容,并将其中的 sec 和 nanosec 字段信息重新组合成一个完整的 timestamp 时间戳,保存到列表 timestamps 中(这里保存 800 组数据)。
  2. 将时间戳两两相减,得到时间间隔信息,保存到列表 time_intervals 中。
  3. 从 time_intervals 列表计算时间间隔的均值、中值、最大值、最小值等指标,可用于衡量抖动情况。
  4. 以时间间隔大小为纵轴,绘制图表,可直观看出抖动情况。

下面是完整代码,这里使用 matplotlib 库绘制图表,因此首先需要引入相关 Python 库。

import matplotlib.pyplot as plt
from datetime import datetime
from matplotlib.font_manager import FontProperties

# 设置中文显示
font = FontProperties(fname='/usr/share/fonts/truetype/wqy/wqy-zenhei.ttc')

file_path = 'ts-data.txt'
timestamps = []

with open(file_path, 'r') as file:
    lines = file.readlines()
    for i in range(len(lines) - 1):  # 避免越界
        if 'sec' in lines[i] and 'nanosec' in lines[i + 1]:
            sec = int(lines[i].split(':')[-1].strip())
            nanosec = int(lines[i + 1].split(':')[-1].strip())
            timestamp = sec + nanosec * 1e-9
            timestamps.append(timestamp)
            if (len(timestamps) > 800):
                break

time_intervals = [timestamps[i + 1] - timestamps[i] for i in range(len(timestamps) - 1)]

mean_interval = sum(time_intervals) / len(time_intervals)
median_interval = sorted(time_intervals)[len(time_intervals) // 2]
max_interval = max(time_intervals)
min_interval = min(time_intervals)

print(f"均值: {mean_interval} 秒")
print(f"中值: {median_interval} 秒")
print(f"最大值: {max_interval} 秒")
print(f"最小值: {min_interval} 秒")

# 绘制时间间隔的图表
plt.figure(figsize=(10, 6))
plt.plot(time_intervals, marker='o', linestyle='-', color='b')
plt.title('ROS Message Time Intervals (with Rviz)')
plt.xlabel('index')
plt.ylabel('interval (s)')
plt.grid(True)
plt.show()

提示:代码和数据可在 GitHub 仓库下载,你也可以参考该示例程序分析其他类似的数据。

输出结果

在 Jupyter Notebook 中点击 Run 执行代码,可以看到如下输入:

均值: 0.1342798188328743 秒
中值: 0.11950397491455078 秒
最大值: 0.7162301540374756 秒
最小值: 0.11851811408996582

时间间隔分布情况如下:

使用 Jupyter 分析 ROS 消息时间间隔抖动数据_第1张图片

将上述 ROS-Message-Time-Intervals.py 程序中的第8行修改为 ts-data-quiet.txt,第37行修改为 plt.title('ROS Message Time Intervals (no Rviz)'),再次点击 Run 重新执行程序。可以看到如下输出:

均值: 0.12153411984443664 秒
中值: 0.1189870834350586 秒
最大值: 0.8321161270141602 秒
最小值: 0.11800003051757812

时间间隔分布情况如下:

使用 Jupyter 分析 ROS 消息时间间隔抖动数据_第2张图片

可以看到,在不启动 Rviz2 的情况下,时间间隔抖动情况有明显改善。

你可能感兴趣的:(ROS,机器人操作系统,jupyter,ide,python,ROS)