【Python, ROS】matplotlibによるリアルタイムのグラフ描画

データの可視化はエンジニアにとって重要なプロセスです。Pythonにmatplotlibという有名なグラフのツールがありますが、ROSメッセージをグラフ化する情報がネット上であまりなかったので、この記事でまとめていきます。

▶English page is here.（英語ページ）

コード

#!/usr/bin/env python

import rospy
from std_msgs.msg import Float64
import numpy as np
import matplotlib.pyplot as plt
import time


start_time = 0.0
t_ = 0.0
y_ = 0.0



def callback(msg):
    print('callback')

    global start_time
    global t_
    global y_

    if start_time>0:
        t_ = time.time() - start_time
        y_ = msg.data

def graph():
    print('realtime_graph')
    rospy.init_node('realtime_graph', anonymous=True)
    rospy.Subscriber("/message", Float64, callback)
    

  #グラフの初期化
    t = [0 for i in range(100)]	#x軸のサイズ
    y = [0 for k in range(100)]	#y軸のサイズ
    
    plt.ion()
    plt.figure()

    plt.title("y")
    plt.xlabel("time[s]")
    plt.ylabel("y[-]")
    plt.ylim(-10, 10)	#固定：y軸の最小値，最大値
    plt.grid(True)
    li, = plt.plot(t, y)
    
    global start_time
    global t_
    global y_

    start_time = time.time()


  #グラフの更新
    while not rospy.is_shutdown():
        print('loop')
        
        t.append(t_)	#x軸で新しい値を追加
        t.pop(0)	#x軸で古い値を消去
        y.append(y_)	#y軸で新しい値を追加
        y.pop(0)	#y軸で古い値を消去

        li.set_xdata(t)
        li.set_ydata(y)
        plt.xlim(min(t), max(t))	#動的：x軸の最小値，最大値

        plt.draw()
        plt.pause(0.1)	#更新周期
    rospy.spin()

if __name__ == '__main__':
    graph()

解説

横軸（x軸）を時間、縦軸（y軸）をsubscribeした値とするグラフを描画するコードを公開しました。

• 最初に（ゼロで）グラフを初期化する必要があります。
• 新しい値を取得したら、それを追加（append）して、一番古い値を消去（pop）します。

さいごに

便利な機能なのでぜひ使ってみてください。

以上です。

追記

こんな大変なコード書かなくても、rqt_plotという便利なROSのツールがありました。