rqt_bag 插件

假设你有一个 bag 文件，想要对其中的数据做自定义可视化。rqt_bag 可以让你滚动浏览录制的消息，并查看原始消息内容。

ros2 run rqt_bag rqt_bag ~/path/to/BagFile
rqt_bag ~/path/to/BagFile                     # 替代方式

默认界面提供了一套标准的可视化方式：

{/截图：标准 rqt_bag 视图/}

但有时候，你可能需要更丰富的视觉效果，或者需要对原始消息做一些后处理。为此，你可以通过 Python 插件系统来编写 rqt_bag 插件，从而实现如下所示的自定义消息可视化：

{/截图：带有彩色时间线和侧边面板的 rqt_bag/}

一些测试数据

在本教程中，我们将使用 diagnostic_msgs/msg/DiagnosticStatus 消息的 level 字段。以下脚本会生成随机级别的诊断状态。你可以基于该脚本录制自己的 bag 文件，也可以直接下载示例数据（解压后使用）。

from diagnostic_msgs.msg import DiagnosticStatus
import random
import rclpy
from rclpy.node import Node

MODES = ['OK', 'WARN', 'ERROR']

class DiagnosticPub(Node):
    def __init__(self):
        super().__init__('diagnostic_pub')
        self.last_status = None
        self.publisher = self.create_publisher(DiagnosticStatus, '/diagnostics', 10)
        self.timer = self.create_timer(1, self.callback)

    def callback(self):
        if self.last_status is None:
            # 随机初始状态
            status = random.randint(0, len(MODES))
        elif random.randint(0, 5) != 0:
            # 不是每个周期都发布消息
            return
        else:
            # 随机新（不同的）状态
            delta = random.randint(1, 2)
            status = (self.last_status + delta) % len(MODES)

        self.get_logger().info(f'Publishing {MODES[status]} status')
        self.publisher.publish(DiagnosticStatus(level=bytes(status)))
        self.last_status = status


def main(args=None):
    rclpy.init(args=args)
    node = DiagnosticPub()
    rclpy.spin(node)


if __name__ == '__main__':
    main()

包设置

我们将创建一个名为 rqt_bag_diagnostics_demo 的包。首先创建一个基本的 ament_python 包，例如运行以下命令：

$ ros2 pkg create --build-type ament_python --dependencies diagnostic_msgs python_qt_binding rqt_bag \
  --description "rqt_bag plugin for diagnostics_msgs" --license Apache-2.0 \
  --maintainer-name "My Name" --maintainer-email "my@name.robots" \
  rqt_bag_diagnostics_demo

将生成的 package.xml 的相关部分修改为如下内容：

<exec_depend>diagnostic_msgs</exec_depend>
<exec_depend>python_qt_binding</exec_depend>
<exec_depend>rqt_bag</exec_depend>
<export>
  <build_type>ament_python</build_type>
  <rqt_bag plugin="${prefix}/plugins.xml"/>
</export>

这里我们让包依赖于 rqt_bag、python_qt_binding 和 diagnostic_msgs，然后导出一个 XML 文件来定义 rqt_bag 插件。接下来在 setup.py 中，将以下行：

('share/' + package_name, ['plugins.xml']),

添加到 data_files 中。

接下来，我们在 plugins.xml 文件中定义插件（即 package.xml 中引用的文件）。该文件描述了本包提供的所有插件（一个包可以包含多个插件）。

<library path=".">
  <class name="DiagnosticBagPlugin"
         type="rqt_bag_diagnostics_demo.the_plugin.DiagnosticBagPlugin"
         base_class_type="rqt_bag::Plugin">
    <description>Awesome Diagnostic</description>
  </class>
</library>

name 属性是插件的名称，必须在所有插件中唯一，除此之外不会在其他地方使用。type 属性指定了 Python 中导入插件类的方式，格式为 package_name.module_name.class_name。

定义插件

现在我们需要实际实现 the_plugin.py 这个 Python 模块（即 plugins.xml 中引用的文件）。首先，确保 rqt_bag_diagnostics_demo 子目录下有一个空的 __init__.py 文件，使其成为一个 Python 包。

注意： 按照当前 ROS 的 Python 包规范，包含 ROS 包的文件夹（rqt_bag_diagnostics_demo）内部会有一个同名的子文件夹。因此完整路径为 WORKSPACE/src/rqt_bag_diagnostics_demo/rqt_bag_diagnostics_demo/__init__.py。

然后在 __init__.py 同目录下创建 the_plugin.py，插件的所有代码都将放在这个文件中。

首先是核心的 Plugin 类。

from rqt_bag.plugins.plugin import Plugin
from python_qt_binding.QtCore import Qt
from diagnostic_msgs.msg import DiagnosticStatus


def get_color(diagnostic):
    if diagnostic.level == DiagnosticStatus.OK:
        return Qt.green
    elif diagnostic.level == DiagnosticStatus.WARN:
        return Qt.yellow
    else:  # ERROR 或 STALE
        return Qt.red


class DiagnosticBagPlugin(Plugin):
    def __init__(self):
        pass

    def get_view_class(self):
        # 此方法是必需的；我们稍后实现
        return None

    def get_renderer_class(self):
        return None

    def get_message_types(self):
        return ['diagnostic_msgs/msg/DiagnosticStatus']

这里包含一些基本的导入语句、一个稍后会用到的辅助函数，以及一个定义了 rqt_bag 插件三个核心部分的类：

1. view_class（即 TopicMessageView）——一个独立的面板，用于查看单个消息。
2. renderer_class（即 TimelineView）——在 bag 数据的时间线视图上进行绘制的工具。
3. message_types——一个字符串数组，指定此插件适用于哪些消息类型。你也可以返回 ['*'] 使其应用于所有消息。

由于前两个方法返回 None，这个插件目前还不会做任何事情。下面我们逐一实现它们。

TopicMessageView

版本 1

我们将创建一个继承自 TopicMessageView 的类（仍然写在 the_plugin.py 中）。首先添加导入：

from rqt_bag import TopicMessageView

然后定义这个新类：

class DiagnosticPanel(TopicMessageView):
    name = 'Awesome Diagnostic'

    def message_viewed(self, bag, entry, ros_message, msg_type_name, topic):
        super(DiagnosticPanel, self).message_viewed(bag=bag, entry=entry, ros_message=ros_message, msg_type_name=msg_type_name, topic=topic)
        print(f'{topic}: {ros_message}')

这里定义了两件事。name 类变量定义了在时间线中右键点击 DiagnosticStatus 话题时 rqt_bag 显示的名称。message_viewed 方法定义了选中某个消息时要执行的操作——目前只是将消息打印到终端。

接下来需要将这个类注册到插件框架中，方法是在 get_view_class 方法中返回类对象本身。

    def get_view_class(self):
        return DiagnosticPanel

注意： 不要写 return DiagnosticPanel()（带括号 ()）。return DiagnosticPanel 才是正确的。

要查看效果，用你的 bag 文件运行 rqt_bag，然后右键点击诊断轨道。在「View」菜单下会出现两个选项：Raw 和我们创建的「Awesome Diagnostic」。点击后应该会打开一个面板，你可以滚动浏览消息并在终端中查看打印输出。

{/截图：rqt_bag 带有空白额外面板/}

版本 2

TopicMessageView 本身继承自 QObject，借助 Qt 的全部功能，你可以实现非常丰富的效果。不过这并非 Python Qt 教程，网上已有丰富的参考资料。因此我们只添加一个简单的 QWidget 并在上面进行绘制。首先添加以下导入：

from python_qt_binding.QtWidgets import QWidget
from python_qt_binding.QtGui import QBrush, QPainter

然后将 DiagnosticPanel 类更新为以下内容：

class DiagnosticPanel(TopicMessageView):
    name = 'Awesome Diagnostic'

    def __init__(self, timeline, parent, topic):
        super(DiagnosticPanel, self).__init__(timeline, parent, topic)
        self.widget = QWidget()
        parent.layout().addWidget(self.widget)
        self.msg = None
        self.widget.paintEvent = self.paintEvent

    def message_viewed(self, bag, entry, ros_message, msg_type_name, topic):
        super(DiagnosticPanel, self).message_viewed(bag=bag, entry=entry,
                                                     ros_message=ros_message, msg_type_name=msg_type_name, topic=topic)
        self.msg = ros_message
        self.widget.update()

    def paintEvent(self, event):
        qp = QPainter()
        qp.begin(self.widget)

        rect = event.rect()

        if self.msg is None:
            qp.fillRect(0, 0, rect.width(), rect.height(), Qt.white)
        else:
            color = get_color(self.msg)
            qp.setBrush(QBrush(color))
            qp.drawEllipse(0, 0, rect.width(), rect.height())
        qp.end()

在构造函数中，我们创建了一个 QWidget 并重写了它的 paintEvent 方法。此后，message_viewed 收到消息时会将其保存并更新 widget，进而触发 paintEvent。请不要手动调用 paintEvent——它必须由 Qt 自行触发。在选中消息之前，我们只绘制一个白色矩形；收到消息后，则通过辅助函数将诊断级别映射为颜色，绘制一个对应颜色的圆。

{/截图：rqt_bag 在额外面板上绘制了圆圈/}

TimelineRenderer

版本 1

要在时间线上绘制内容，我们继承 TimelineRenderer 类（同样写在 the_plugin.py 中）。添加导入：

from rqt_bag import TimelineRenderer

然后添加新类。

class DiagnosticTimeline(TimelineRenderer):
    def __init__(self, timeline, height=80):
        TimelineRenderer.__init__(self, timeline, msg_combine_px=height)

    def draw_timeline_segment(self, painter: QPainter, topic, start: float, end: float, x: float, y: int, width: float, height: int):
        painter.setBrush(QBrush(Qt.blue))
        painter.drawRect(int(x), y, int(width), height)

你可以通过 msg_combine_px 参数来自定义消息在时间线中的高度。需要重写的核心方法是 draw_timeline_segment()，它会将时间线的各段区域交给你来绘制。目前我们只是简单地在每个段上画一个蓝色矩形。

与消息视图一样，还需要修改插件，让它返回这个类。

    def get_renderer_class(self):
        return DiagnosticTimeline

要查看效果，你需要在 rqt_bag 界面中启用「Thumbnails」（这个名称有些误导性）。

{/截图：rqt_bag 时间线上绘制蓝色条/}

版本 2

接下来，我们希望根据消息内容来自定义时间线上的绘制方式。为此，需要从 bag 文件中读取并反序列化消息。先添加以下导入：

from python_qt_binding.QtGui import QPen
from rclpy.time import Time
from rclpy.serialization import deserialize_message
from rqt_bag.bag_helper import to_sec

然后更新 draw_timeline_segment()：

    def draw_timeline_segment(self, painter: QPainter, topic, start: float, end: float, x: float, y: int, width: float, height: int):
        bag_timeline = self.timeline.scene()
        start_t = Time(seconds=start)
        end_t = Time(seconds=end)

        for bag, entry in bag_timeline.get_entries_with_bags([topic], start_t, end_t):
            topic, raw_data, t = bag_timeline.read_message(bag, entry.timestamp, topic)
            msg = deserialize_message(raw_data, DiagnosticStatus)
            color = get_color(msg)
            painter.setBrush(QBrush(color))
            painter.setPen(QPen(color, 5))

            t_float = to_sec(Time(nanoseconds=t))
            p_x = int(self.timeline.map_stamp_to_x(t_float))
            painter.drawLine(p_x, y, p_x, y + height - 1)

利用 draw_timeline_segment() 方法接收的 topic、start 和 end 参数，我们可以获取与时间线该段对应的 bag 条目，进而取出实际消息进行绘制。这里我们根据诊断消息的级别绘制一条彩色的线。map_stamp_to_x() 方法可以将浮点秒数转换为 widget 像素坐标，帮我们自动确定消息的水平绘制位置。

{/截图：rqt_bag 时间线上绘制不同颜色的条/}

如果时间线上计算消息表示的开销较大，你可以参考 ImageTimelineViewer 的做法，使用 Timeline Cache 来优化性能。具体实现方式就留作练习了。