ROS講座55 pythonでrosbagを解析・可視化する

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |
| python | 3.8.10 |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
rosbagを見るにはrqt_bagなどのツールがあります。中身を詳細に見るにはxmlに出力してExcelに読み込むこともありますが面倒です。解析はpythonの得意分野なのでpythonで直接rosbagを読み込んで可視化する方法を解説します。

# 使用するpythonパッケージ

* rosbag（rosのインストール時に入る）
* numpy
* matplotlib
* import sys
* import os

rosobagはpython2系だけが対応です（2019年1月現在）。

# ソースコード
今回は以下のようなディレクトリ構成でファイルがあることを想定します

```text:ファイル配置
py_lecture
├ resources
│   └ robot_bag.bag
└ scripts
    └ rosbag_analisys_pose.py
```

bagファイルに入っている`/pose`という名前の`geometry_msgs::Pose`型のトピックを可視化します。

## pythonスクリプト
```python:py_lecture/scripts/rosbag_analisys_pose.py
#!/usr/bin/env python3
import rosbag
import numpy as np
import matplotlib.pyplot as plt
import sys
import os

args = sys.argv
print(len(args))
assert len(args)>=2, "you must specify the argument."

# get path
filename=os.path.normpath(os.path.join(os.getcwd(),args[1]))
print(filename)

# read from bag file
bag = rosbag.Bag(filename)
np_poses=None
for topic, msg, t in bag.read_messages():
    if topic=="/pose":
	np_pose=np.array([[0.0, 0.0, 0.0, 0.0, 0.0]])
        np_pose[0,0]=msg.position.x
        np_pose[0,1]=msg.position.y
        np_pose[0,2]=msg.position.z
        np_pose[0,3]=t.secs
        np_pose[0,4]=t.nsecs
        if np_poses is None:
            np_poses=np_pose
        else:
            np_poses=np.append(np_poses,np_pose,axis=0)

# reform time
start_sec=np_poses[0,3]
start_nsec=np_poses[0,4]
t=np.zeros(np_poses.shape[0],dtype='float32')
for i in range(np_poses.shape[0]):
    t[i]=(np_poses[i,3]-start_sec)+(np_poses[i,4]-start_nsec)/1000000000.0

# plot    
plt.subplot(121)
plt.title("time vs x,y")
plt.plot(t, np_poses[:,0], 'r', label="x")
plt.plot(t, np_poses[:,1], 'b', label="y")
plt.xlabel("time[s]")
plt.ylabel("vel[m/s]")
plt.legend()

plt.subplot(122)
plt.title("x vs y")
plt.plot(np_poses[:,0], np_poses[:,1], 'g')
plt.show()

bag.close()
```

## 引数の解決、パスの解決

* `args = sys.argv`で引数を受け取ります。引数が無い場合はassartするようにしています。
* そのあとはbagファイルのある場所の解決をしています。
    * os.getcwd()はカレントディレクトリを取得します。
    * os.path.join()は2つのパスをくっつけます。
    * os.path.normpath()はパスの正規化をします。`A//B`や`A/foo/../B`などを`A/B`に正規化します。 

## bagファイルの読み込み
`bag = rosbag.Bag(filename)`でbagfileを読み込みます。
`for topic, msg, t in bag.read_messages():`でbagに記録されているROSトピックを1つ1つ読み出します。
topicにはトピック名（例：/pose）、msgにはそのトピックの中身の構造体が、tにはそのトピックを取得したときの時間が入ります。

## 時間の整形
ROSの時間はunix時間で管理されているので非常に大きいものになります。これだとわかりにくいので、一番最初のトピックが来た時を基準に時間を取り直します。ROSの時間はunix時間をそのままとるsecsと小数点以下をナノ秒単位でとるnsecsの組で表されます。

```python:時間の整形部分
start_sec=np_poses[0,3]
start_nsec=np_poses[0,4]
t=np.zeros(np_poses.shape[0],dtype='float32')
for i in range(np_poses.shape[0]):
    t[i]=(np_poses[i,3]-start_sec)+(np_poses[i,4]-start_nsec)/1000000000.0
```

# 実行
まずscriptsディレクトリに移動します。そのあとにpythonスクリプトを実行します。引数としてはカレントディレクトリからbagファイルまでの相対パスを入れます。

```shell
cd ~/catkin_ws/src/ros_lecture/py_lecture/scripts/
python3 rosbag_analisys_pose.py  ../resources/robot_bag.bag 
```

すると以下のようなグラフが描画されます。
![py_plot1.png](https://qiita-image-store.s3.amazonaws.com/0/254442/6cfac88b-a35a-8a16-fa6a-b4f4ea1fdd7f.png)


# 参考
[roswiki: rosbagをpythonで使う](http://wiki.ros.org/rosbag/Cookbook)
[matplotlibの使い方](http://bicycle1885.hatenablog.com/entry/2014/02/14/023734)
[グラフの体裁を整える](https://yubais.net/doc/matplotlib/modify.html)
[コマンドライン引数の扱い](https://qiita.com/orange_u/items/3f0fb6044fd5ee2c3a37)
[pythonパス操作](https://docs.python.jp/3/library/os.path.html#os.path.expanduser)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)