やりたいこと
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をおいてやってください。
以下コードです。
#!/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とかいうファイルができてます。
以上です。