-
Notifications
You must be signed in to change notification settings - Fork 1
/
ms25_euler_to_rosbag.py
61 lines (42 loc) · 1.29 KB
/
ms25_euler_to_rosbag.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
# !/usr/bin/python
#
# Convert the ms25_euler csv files to a rosbag
#
# To call:
#
# python ms25_euler_to_rosbag.py ms25_euler.csv ms25_euler.bag
#
import rosbag, rospy
from std_msgs.msg import Float64MultiArray, MultiArrayDimension, MultiArrayLayout
import sys
import numpy as np
def main(args):
if len(sys.argv) < 2:
print 'Please specify ms25_euler file'
return 1
if len(sys.argv) < 3:
print 'Please specify output rosbag file'
return 1
ms25_euler = np.loadtxt(sys.argv[1], delimiter = ",")
utimes= ms25_euler[:, 0]
rs = ms25_euler[:, 1]
ps = ms25_euler[:, 2]
hs = ms25_euler[:, 3]
bag = rosbag.Bag(sys.argv[2], 'w')
try:
for i, utime in enumerate(utimes):
timestamp = rospy.Time.from_sec(utime/1e6)
layout_rph = MultiArrayLayout()
layout_rph.dim = [MultiArrayDimension()]
layout_rph.dim[0].label = "rph"
layout_rph.dim[0].size = 3
layout_rph.dim[0].stride = 1
euler = Float64MultiArray()
euler.data = [rs[i], ps[i], hs[i]]
euler.layout = layout_rph
bag.write('ms25_euler', euler, t=timestamp)
finally:
bag.close()
return 0
if __name__ == '__main__':
sys.exit(main(sys.argv))