環境
この記事は以下の環境で動いています。
| 項目 | 値 |
|---|---|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |
| python | 3.8.10 |
インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。
概要
rosbagを見るにはrqt_bagなどのツールがあります。中身を詳細に見るにはxmlに出力してExcelに読み込むこともありますが面倒です。解析はpythonの得意分野なのでpythonで直接rosbagを読み込んで可視化する方法を解説します。
使用するpythonパッケージ
- rosbag（rosのインストール時に入る）
- numpy
- matplotlib
- import sys
- import os
rosobagはpython2系だけが対応です（2019年1月現在）。
ソースコード
今回は以下のようなディレクトリ構成でファイルがあることを想定します
py_lecture
├ resources
│ └ robot_bag.bag
└ scripts
└ rosbag_analisys_pose.py
bagファイルに入っている/poseという名前のgeometry_msgs::Pose型のトピックを可視化します。
pythonスクリプト
#!/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の組で表されます。
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ファイルまでの相対パスを入れます。
cd ~/catkin_ws/src/ros_lecture/py_lecture/scripts/
python3 rosbag_analisys_pose.py ../resources/robot_bag.bag
すると以下のようなグラフが描画されます。
参考
roswiki: rosbagをpythonで使う
matplotlibの使い方
グラフの体裁を整える
コマンドライン引数の扱い
pythonパス操作