ROSmsgをmatplotlibでリアルタイム描画【rospy】

私の知る限り、ROSのメッセージをリアルタイムで描画する方法は以下の2つです。

• matplotlibで実装
• rqt_plotを使用

特にこだわりがなければrqt_plotが便利です。しかし、軸、凡例などをカスタマイズするためには、matplotlibを利用して自身で実装する必要があります。

この記事では、matplotlibを用いた実装例をご紹介します。

実装の概要

• サブスクライブした角度（ロール、ピッチ、ヨー）を表示
• Subscribe：geometry_msgs.msg.Vector3Stamped
• Publish：なし
• 可視化：matplotlib.pyplot
  • グラフx3（ロール、ピッチ、ヨー）
  • 横軸：時間[s]
  • 縦軸：角度[deg]

ソースコード

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Vector3Stamped

import math
import time
import matplotlib.pyplot as plt

class PlottingRPY:
    def __init__(self):
        ## subscriber
        self.sub_vector = rospy.Subscriber("/rpy", Vector3Stamped, self.callbackVector)
        ## msg
        self.rpy = Vector3Stamped()
        ## list
        self.list_t = []
        self.list_r = []
        self.list_p = []
        self.list_y = []
        ## line
        self.line_r = None
        self.line_p = None
        self.line_y = None
        ## time
        self.start_time = time.time()
        ## parameter
        self.interval = 0.1
        self.ylim = 180.0
        self.shown_size = 100

        ## initialization
        self.initializePlot()
        ## loop
        self.mainLoop()

    def callbackVector(self, msg):
        self.rpy = msg

    def initializePlot(self):
        plt.ion()	#interactive mode on
        plt.figure()
        ## empty list
        self.list_t = [0 for i in range(self.shown_size)]
        self.list_r = [0 for i in range(self.shown_size)]
        self.list_p = [0 for i in range(self.shown_size)]
        self.list_y = [0 for i in range(self.shown_size)]
        ## roll
        plt.subplot(3, 1, 1)
        # plt.title("roll")
        plt.xlabel("time[s]")
        plt.ylabel("roll[deg]")
        plt.ylim(-self.ylim, self.ylim)	#軸の最小値、最大値
        plt.grid(True)
        self.line_r, = plt.plot(self.list_t, self.list_r)	#ラインの取得
        ## pitch
        plt.subplot(3, 1, 2)
        # plt.title("pitch")
        plt.xlabel("time[s]")
        plt.ylabel("pitch[deg]")
        plt.ylim(-self.ylim, self.ylim)	#軸の最小値、最大値
        plt.grid(True)
        self.line_p, = plt.plot(self.list_t, self.list_p)	#ラインの取得
        ## yaw
        plt.subplot(3, 1, 3)
        # plt.title("yaw")
        plt.xlabel("time[s]")
        plt.ylabel("yaw[deg]")
        plt.ylim(-self.ylim, self.ylim)	#軸の最小値、最大値
        plt.grid(True)
        self.line_y, = plt.plot(self.list_t, self.list_y)	#ラインの取得

    def mainLoop(self):
        while not rospy.is_shutdown():
            self.updatePlot()
            self.drawPlot()

    def updatePlot(self):
        ## append
        t = time.time() - self.start_time
        self.list_t.append(t)
        self.list_r.append(self.rpy.vector.x/math.pi*180.0)
        self.list_p.append(self.rpy.vector.y/math.pi*180.0)
        self.list_y.append(self.rpy.vector.z/math.pi*180.0)
        ## pop
        self.list_t.pop(0)
        self.list_r.pop(0)
        self.list_p.pop(0)
        self.list_y.pop(0)
        ## roll
        plt.subplot(3, 1, 1)
        self.line_r.set_xdata(self.list_t)	#ラインの書き換え（x軸）
        self.line_r.set_ydata(self.list_r)	#ラインの書き換え（y軸）
        plt.xlim(min(self.list_t), max(self.list_t))	#時間軸の最小値、最大値
        ## pitch
        plt.subplot(3, 1, 2)
        self.line_p.set_xdata(self.list_t)	#ラインの書き換え（x軸）
        self.line_p.set_ydata(self.list_p)	#ラインの書き換え（y軸）
        plt.xlim(min(self.list_t), max(self.list_t))	#時間軸の最小値、最大値
        ## yaw
        plt.subplot(3, 1, 3)
        self.line_y.set_xdata(self.list_t)	#ラインの書き換え（x軸）
        self.line_y.set_ydata(self.list_y)	#ラインの書き換え（y軸）
        plt.xlim(min(self.list_t), max(self.list_t))	#時間軸の最小値、最大値

    def drawPlot(self):
        ## draw
        plt.draw()
        plt.pause(self.interval)

def main():
    rospy.init_node('plotting_rpy', anonymous=True)
    plotting_rpy = PlottingRPY()
    rospy.spin()

if __name__ == '__main__':
    main()

実装の解説

処理の流れ

• 最初にゼロのリストでグラフを初期化
• 新しいメッセージを取得したときに
  • それを追加（append）
  • 一番古い値を消去（pop）
• ラインを書き換える
• 表示

ラインについて

matplotlibで表示している折れ線の正体はmatplotlib.lines.Line2Dというものらしいです。これを書き換えることでグラフを更新します。

• 取得
  

  self.line_r, = plt.plot(self.list_t, self.list_r)

• 書き換え
  

  self.line_r.set_xdata(self.list_t)
  self.line_r.set_ydata(self.list_r)

補足

matplotlib.pyplotはメインループでないと機能しないそうです。例えばコールバック関数内でplt.pause()を実行すると以下のようなエラーが出てしまいます。

RuntimeError: main thread is not in main loop

さいごに

参考になれば幸いです。

以上です。