mavrosのIMUデータをクォータニオンからオイラー角に変換してCSVに出力する

#やりたいこと
pixhawkとKinectV2を積んだロボットで、画像を解析する際に姿勢も知りたい。

##方法
pixhawkから出力されるIMUデータをmavrosを介してサブスクライブしクォータニオンからオイラー角に変換してCSVに出力する。

##なぜわざわざ変換してCSVに出力するのか
rosbagでKinectv2の画像とpixhawkからのIMUデータを記録したのだが、このbagファイルを解析しようとした時に、Kinectとpixhawkが吐くデータの周波数が異なっていたため、一旦CSVに出力してタイムスタンプが近いデータだけ抜き取って扱いたかった。

IMUの姿勢はクォータニオンで表されているので、扱いやすくするためオイラー角に変換したかった。

ubuntuの表計ソフト上（LibreOffice Calc）でクォータニオンからオイラー角に変換するのが面倒だったので、pythonで変換とCSVへの出力を同時に行うROSのノードを書いた訳です。

##準備
※既にmavros環境が整っている前提で進めます。

適当にパッケージを作成します。

コマンド

cd catkin_ws/src
catkin_create_pkg transform
cd transform/src
mkdir scripts

scripts以下にeuler_from_quaternion.pyをおいてやってください。
以下コードです。

euler_from_quaternion.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import math
import csv
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion

header = ["seq", "secs", "nsecs", "roll_rad", "pitch_rad", "yaw_rad"]

def callback(data):
    quaternion = (data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w)
    seq = data.header.seq
    secs = data.header.stamp.secs
    nsecs = data.header.stamp.nsecs

    euler = euler_from_quaternion(quaternion)
    roll_rad = euler[0]
    pitch_rad = euler[1]
    yaw_rad = euler[2]

    roll =  math.degrees(roll_rad)
    pitch = math.degrees(pitch_rad)
    yaw = math.degrees(yaw_rad)

    rospy.loginfo(rospy.get_caller_id() + "\nqhd_color_time:\nseq: [{}]\nsecs: [{}]\nnsecs: [{}]"
    .format(data.header.seq, data.header.stamp.secs, data.header.stamp.nsecs))

    print "roll: %f" %(roll)
    print "pitch: %f" %(pitch)
    print "yaw: %f" %(yaw)

    body = [seq, secs, nsecs, roll_rad, pitch_rad, yaw_rad]
    f = open('euler.csv', 'a')
    writer = csv.writer(f)
    writer.writerow(body)
    f.close()

def listener():
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber("/mavros/imu/data", Imu, callback)
    rospy.spin()

if __name__ == '__main__':
    f = open('euler.csv', 'a')
    writer = csv.writer(f)
    writer.writerow(header)
    f.close()
    listener()

なんか冗長的ですが多分使えます。print文とかrospy.loginfoの部分は正直いらないです。

権限を与え忘れないように、私は今でも忘れます

コマンド

chmod +x euler_from_quaternion.py
source ~/.bashrc

準備は以上です。

##実際にやってみます
ターミナル3つほど開いて、次の順にコマンドを実行してください。

コマンド

 roscore

コマンド

 rosrun transform euler_from_quaternion.py

コマンド

 rosbag play -r 30 *.bag

rosbagの再生時にオプションとして
-r 30　
をつけているのは30倍速で再生という意味です。ここは任意の数字で構いません。

おそらくホームディレクトリにeuler.csvとかいうファイルができてます。
以上です。