65 lines
1.6 KiB
Python
Executable File
65 lines
1.6 KiB
Python
Executable File
#!/usr/bin/env python
|
|
#
|
|
# BSD 3-Clause License
|
|
#
|
|
# This file is part of the Basalt project.
|
|
# https://gitlab.com/VladyslavUsenko/basalt.git
|
|
#
|
|
# Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel.
|
|
# All rights reserved.
|
|
#
|
|
|
|
import sys
|
|
import os
|
|
import rospy
|
|
import argparse
|
|
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__':
|
|
|
|
parser = argparse.ArgumentParser(description='Record Motion Capture messages from ROS (/vrpn_client/raw_transform).')
|
|
parser.add_argument('-d', '--dataset-path', required=True, help="Path to store the result")
|
|
args = parser.parse_args()
|
|
|
|
dataset_path = args.dataset_path
|
|
|
|
out_file = None
|
|
time_offset = None
|
|
|
|
if not os.path.exists(dataset_path):
|
|
os.makedirs(dataset_path)
|
|
|
|
out_file = open(dataset_path + '/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()
|
|
|