ROS topicのプロット方法色々

概要

ROSのトピックのデータからグラフを描くとき，色々なやり方があります．
今回紹介するのは以下の方法です．

• rqt_plot
• rqt_bag
• plot_juggler
• csv形式にする（その後にExcelなどで処理する）
• rosbagAPI+matplotlib(Python)

それぞれの特徴は以下のようになっています．

*印については，GUIであるExcelやLibreOffice Calcで扱いやすくするために，CUIでcsv形式にするのでGUIとCUIの中間に入れました．

まず全体に対して言えることは，GUIは傾向の大枠を掴みたいときに最初にざっと見る確認に適していて，CUIで処理するのはたくさんのファイルをまとめて処理・それぞれのグラフを見比べながらデータ解析をしたい場合に適していると思います．
ちなみに筆者のオフラインデータ処理のおすすめは，最初に確認程度に数ファイルだけplotjuggler→そのあとにrosbagAPI+matplotlibでファイルをたくさん処理する方法です．

次のセクションから，それぞれのプロット方法について特徴を紹介します．
GUIツールに関しては，細かい使い方は公式Docを参照ください．
プログラムを要するものはサンプルコードを記載しました．

rqt_plot

ROS Wiki: rqt_plot

できること

• リアルタイムにトピックをプロットすることができる．
• 同様にrosbagの再生をしながら時間経過とともに値の変化を確認できる．

▽rqt_plotの画面(そのうちgif動画をあげます)

rqt_bag

ROS Wiki: rqt_bag

できること

• トピックのheaderのtimestampに基づいてプロットすることができる．
• bagfileに含まれている全部のデータのpublishタイミングもみることができる．

できないこと

• 一つのトピックのデータならば重ねてプロットできるが，複数トピックを重ねる機能はない．（rosbagのトピック全部まとめたものを新たに一つのトピックとして出せばいいんじゃないか？（友人談））
• 前回の設定を引き継いでプロットすることができないので，新規ファイルを読み込むたびに全部クリックして設定する必要があり面倒．

▽全トピックのpublishのタイミングを一気に確認できる様子．

▽一つのトピックに含まれているデータならば，チェックボックスをクリックするだけで複数の値を重ねてプロットできる．（画像は一つしか描画していない時のもの．画像加工がめんどくさかったのでこの画像でお許しください…）

Plot Juggler

ROS Wiki: plotjuggler

できること

• 複数トピックを重ねてプロットすることができる．
• 画面分割が簡単．
• ドラッグ＆ドロップで直感的に操作が可能．
• 別のファイルをロードするときに，直前のファイルをロードするときに選んだトピックが既に選択されていて，ロードが簡単．
• plotjugglerを起動したまま，別のファイルをロードしたら，直前のファイルと同じ設定のままプロットしてくれる．

できないこと

• rosbagがトピックを受け取ったときの時間に応じてしかプロットができないので，通信遅延のあるデータだとなめらかにプロットができない．

▽読み込むトピックを選べる（全トピックでもいい．少ない数のトピックを選んでも次回読み込み時に同じものが既にクリックされた状態になっている．）

▽画面分割して，トピックのプロットしたい値をドラッグ＆ドロップで簡単に表示することができる．

▽通信環境が悪いときに録ったrosbagだと，なめらかにプロットができない．左：通信遅延無し，右：通信遅延あり．（通信再開時にキューに溜まっていたトピックが同じ時間軸に複数プロットされてしまう．）

csvにしてエクセルなどで処理する

シェルスクリプトでrosbagに記録されているトピックのなかから，プロットしたいトピックを.csv形式にします．
私はcsv形式にしてからはExcel, LibreOffice Calc,gnu-plotのいずれかを利用してプロットしていました．

できること

• csv形式にさえしてしまえば処理の仕方はあなた次第．

できないこと

• rostopic echoからcsvに書き出す方法では一トピック一ファイルになってしまい，複数トピックをまとめられない．(timestampがバラバラなので統合は面倒．csvにしてから統合するプログラムを書いたけど，そこまでやるならrosbagAPIを使うべきだった)

ここでは自分でシェルスクリプトを書く場合と，csvに変換してくれるツールを紹介します．

方法１　シェルスクリプトを書いてcsvに変換する

以下は

1. 指定したフォルダ配下にある.rosbagを全部(サブフォルダ内も)探して，
2. /imu/dataと/position/poseトピックを.csv形式でそれぞれcsv_imu，csv_positionフォルダに保存

するシェルスクリプトのサンプルコードです．

シェルスクリプトのサンプルコード（クリックで展開）

dataprocess.sh

#!/bin/sh

### 使い方１　まずはターミナルでchmod +x dataprocess.sh で実行可能にする．
### 使い方２  実行するときは”./dataprocess.sh ~/処理したいrosbagのあるフォルダのパス/”
### 第一引数には処理したいbagファイルのありかを書こう！
### 指定したディレクトリ配下にあるbagファイルが表示されて正しいか確認したらyesのyを押そう！

# 第一引数で指定したディレクトリ配下のテキストファイルを一覧表示する。
echo "Files to be processed are..."
for file in `find $1 -name "*.bag"`; do
  echo " $file"
done!


# 大丈夫？本当にこれらのbagfileで合ってるよね？っていう確認．
echo "Is it ok to continue? (type y/n)"
read INPUT

if [ "$INPUT" = "y" ]; then
  echo "Generating csv files..."

  for file in `find $1 -name "*.bag"`; do
    # パスの部分だけ
    path=$(dirname "${file}")
    # ファイル名を取り出す（拡張子あり）
    filename="${file##*/}"
    # 拡張子をなくす
    fname="${filename%.*}"
    mkdir -p ${path}/csv_imu
    mkdir -p ${path}/csv_position
    rostopic echo -b $file -p /imu/data > "${path}/csv_imu/csv_${fname}".csv
    rostopic echo -b $file -p /position/pose > "${path}/csv_position/csv_${fname}".csv
  done

elif [ "$INPUT" = "n" ];then
  echo "Quit"
else
  echo "type y/n"
fi

私はシェルスクリプト内でトピック名を指定したり，bagファイル全部を処理したりしていますが，以下をやるともっと便利です．

• 引数でトピック名を指定できるようにする
• rosbagのファイル名にxxxxが含まれている場合だけ処理する

方法２　rosbagからcsvに変換してくれるパッケージを使う

bag2csv.pyというパッケージでrosbagからcsvに簡単に変換できます．

▽このようになるそうです．画像引用元:[2]

bag2csv.pyは、bagファイルの中にあるデータをcsvファイルに変換するツールです。
使い方は、python bag2csv.py -t /huge/output hoge.bagと-tの後に出力したいトピック名と、その後にbagファイルを指定すれば、Convertedbag.csvという変換されたファイルが冒頭のスクリーンショットのようにできるはずです。(引用元:[2])

参考: [2]

rosbagAPI+matplotlib

rosbagAPIではPythonかC++でrosbagのデータを処理することができます．

できること

• GUIではできなかった細かい指定など，なんでもできる．

私はPythonでrosbagAPIとプロットのためのライブラリ，matplotlibの組み合わせで使ったので，その方法を紹介します． 以下は

1. 引数で指定されたフォルダパスからrosbagを探し，
2. /cmd_velトピックを受け取った後から，/pose_infoトピックのpos_x,pos_yをプロットして，
3. pngで出力 or 画面上に表示する

プログラムです．

rosbagAPIとmatplotlibのサンプルコード（クリックで展開）

pos_plot.py

#!/usr/bin/env python
## coding: UTF-8
# 参考： ROS講座55 pythonでrosbagを解析・可視化する https://qiita.com/srs/items/4d19a749891728c8520a
import rosbag
import numpy as np
import sys
import os
from enum import IntEnum

# window = True # 画面に表示したいときはこちらを，
window = False # 画面に表示しないでpng出力したいときはこちらを使う
if not window:
    import matplotlib
    matplotlib.use('Agg') # 画面に表示しないでファイルとして出力する
import matplotlib.pyplot as plt

class PlotClass():

    output_name = None
    outputpath = None
    np_pos = None
    cmd_vel = False # cmd_velが発行されたあとにTrueにする
    prev_row = None

    class PosNum(IntEnum):
        POS_X = 0
        POS_Y = 1
        POSITION_SEC = 2
        POSITION_NSEC = 3

    def __init__(self, input_path, output_dir , file):
        file_path = os.path.join(input_path, file)
        print("Processing  " + file_path)
        self.output_name = os.path.splitext(file)[0]
        if not window:
            self.outputpath = output_dir + "/" + self.output_name + ".png"
        bag = rosbag.Bag(file_path)
        for topic, msg, t in bag.read_messages():

            self.read_topic(topic, msg, t)

        # reform time (そのままだとUNIX epoch timeでめっちゃ長い)
        start_sec = self.np_pos[0, int(self.PosNum.POSITION_SEC)]
        start_nsec = self.np_pos[0, self.PosNum.POSITION_NSEC]
        t = np.zeros(self.np_pos.shape[0], dtype = 'float32')
        for i in range(self.np_pos.shape[0]):
            t[i] = (self.np_pos[i, self.PosNum.POSITION_SEC] - start_sec) + (self.np_pos[i, self.PosNum.POSITION_NSEC] - start_nsec) / 1000000000.0

        self.plot_pos(t)
        bag.close()


    def read_topic(self, topic, msg, t):

        if topic == "/cmd_vel":
            self.cmd_vel = True

        elif topic=="/pose_info":

            pos_row = np.array([[0.0, 0.0, 0.0, 0.0]])
            pos_row[0, self.PosNum.POS_X] = msg.pos_x
            pos_row[0, self.PosNum.POS_Y] = msg.pos_y
            # pos_row[0, 2] = t.secs # こっちはrosbagに記録された時間なので遅延があると変なグラフになる
            # pos_row[0, 3] = t.nsecs
            pos_row[0, self.PosNum.POSITION_SEC] = msg.header.stamp.secs # Pythonだとsecsなので注意
            pos_row[0, self.PosNum.POSITION_NSEC] = msg.header.stamp.nsecs 
            
            ## << 以下はグラフが変化しない部分もプロットするとき
            # if self.np_pos is None:
            #     self.np_pos = pos_row
            # else:
            #     self.np_pos = np.append(self.np_pos, pos_row, axis = 0) 
            ##                      グラフが変化しない部分もプロットするとき >>


            ## << cmd_velが発行された後だけをプロットするとき
            if self.cmd_vel == False: # まだcmd_velがないとき
                self.prev_row = pos_row
            else:
                if self.np_pos is None:# 初期値の場合はそのまま代入
                    self.np_pos = self.prev_row # cmd_velがonになる直前の値を入れる

                # Append:配列末尾に要素を追加した新しい配列を生成する関数．
                # 引数(要素を追加したい配列，追加する要素or配列，指定した軸方向に沿って要素を追加する) <- 行ごとに要素を追加していく
                self.np_pos = np.append(self.np_pos, pos_row, axis = 0) 
            ##                  cmd_velが発行された後だけをプロットするとき>>
            
    def plot_pos(self, t):

        # 画面に表示，もしくはファイルとして出力する．windowをTrueFalseにすることで切り替えられる
        title = "time vs position (" + self.output_name + ")"
        label_x = "time[s]"
        label_y = "position[m]"

        plt.figure()
        plt.plot(t, self.np_pos[:, self.PosNum.POS_X], 'r', label = "pos_x")
        plt.plot(t, self.np_pos[:, self.PosNum.POS_Y], 'b', label = "pos_y")
        plt.xlim(0, 25)
        plt.ylim(0, 30)
        plt.title(title)
        plt.xlabel(label_x, fontsize=25)
        plt.ylabel(label_y, fontsize=25)
        plt.legend()
        plt.grid(which = "major", axis = "x", color = "black", alpha = 0.8, linestyle = "--", linewidth = 1) # グリッドの設定
        plt.grid(which = "major", axis = "y", color = "black", alpha = 0.8, linestyle = "--", linewidth = 1)
        plt.tick_params(labelsize=18)
        plt.tight_layout()

        if window:
            plt.show()

        else:
            plt.savefig(self.outputpath)
            print("saving figure to " + self.outputpath)

def main():
    args = sys.argv
    print(len(args))
    assert len(args)>=2, "You must specify the directory path which contains rosbag files as argument."

    # get path
    input_path = os.path.normpath(args[1]) # 入力のディレクトリパス

    output_dir = None
    if not window:
        output_dir = input_path + '/graph_position' # png出力の場合の出力先フォルダ
        if not os.path.exists(output_dir):
            os.makedirs(output_dir) # 出力先フォルダがなければ作る

    for file in os.listdir(input_path):
        if file.endswith(".bag"): # rosbag全部のループ
            plotclass = PlotClass(input_path, output_dir, file) # インスタンス生成
            
        # break # DEBUG用

if __name__ == "__main__":
    main()

▽こんなグラフが生成されます（適当なデータなので全然ポジションじゃないです．pos_yはpos_xシフトしただけのデータ．）

参考：[1]

bag_plotter.pyを使う（追記）

bag_plotter.pyはbagファイルから直接グラフを作成するツールです。
yamlファイルにグラフの設定を書いて、そのyamlファイルとbagファイルを読みこませることで、データをプロットすることができます。(引用元:[2])

▽こんな風にプロットできるそうです．(画像引用元：[2])

参考；[2]

おわりに

筆者が「できないこと」と思っている機能が，本当は既にあるのに気がついていないだけの可能性もあるので，もし「これはできるよ！」というのがあったらご指摘ください．

Reference

[1] ROS講座55 pythonでrosbagを解析・可視化する
[2] 東大JSKが公開しているクールなROS可視化ライブラリを使ってみる

追記

Date	Contents
2019.12.15	rosbagからcsvに変換してくれるツールの追加 rosbagからプロットしてくれるパッケージの追加