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

15810 ワード
Python ロボット ROS Python テキストリンク

環境

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

項目 値
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スクリプト

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の組で表されます。

時間の整形部分
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パス操作

目次ページへのリンク

ROS講座の目次へのリンク