48 lines
1.1 KiB
Python
48 lines
1.1 KiB
Python
|
#!/usr/bin/env python
|
||
|
|
|
||
|
|
import sys
|
||
|
|
import os
|
||
|
|
import rospy
|
||
|
|
from geometry_msgs.msg import TransformStamped
|
||
|
|
|
||
|
|
|
||
|
|
def callback(data):
|
||
|
|
global out_file, time_offset
|
||
|
|
if not out_file:
|
||
|
|
return
|
||
|
|
|
||
|
|
if not time_offset:
|
||
|
|
time_offset = rospy.Time().now() - data.header.stamp
|
||
|
|
|
||
|
|
out_file.write('{},{},{},{},{},{},{},{}\n'.format(
|
||
|
|
data.header.stamp + time_offset,
|
||
|
|
data.transform.translation.x,
|
||
|
|
data.transform.translation.y,
|
||
|
|
data.transform.translation.z,
|
||
|
|
data.transform.rotation.w,
|
||
|
|
data.transform.rotation.x,
|
||
|
|
data.transform.rotation.y,
|
||
|
|
data.transform.rotation.z
|
||
|
|
))
|
||
|
|
|
||
|
|
|
||
|
|
def listener():
|
||
|
|
rospy.init_node('listener', anonymous=True)
|
||
|
|
rospy.Subscriber('/vrpn_client/raw_transform', TransformStamped, callback)
|
||
|
|
|
||
|
|
rospy.spin()
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
out_file = None
|
||
|
|
time_offset = None
|
||
|
|
|
||
|
|
if not os.path.exists(sys.argv[1]):
|
||
|
|
os.makedirs(sys.argv[1])
|
||
|
|
|
||
|
|
out_file = open(sys.argv[1] + '/data.csv', 'w')
|
||
|
|
out_file.write('#timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z []\n')
|
||
|
|
listener()
|
||
|
|
out_file.close()
|
||
|
|
|