-
Notifications
You must be signed in to change notification settings - Fork 1
/
ms25_to_rosbag.py
85 lines (61 loc) · 1.94 KB
/
ms25_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
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
# !/usr/bin/python
#
# Convert the ms25 csv files to a rosbag
#
# To call:
#
# python ms25_to_rosbag.py ms25.csv ms25.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 file'
return 1
if len(sys.argv) < 3:
print 'Please specify output rosbag file'
return 1
ms25 = np.loadtxt(sys.argv[1], delimiter = ",")
utimes= ms25[:, 0]
mag_xs = ms25[:, 1]
mag_ys = ms25[:, 2]
mag_zs = ms25[:, 3]
accel_xs = ms25[:, 4]
accel_ys = ms25[:, 5]
accel_zs = ms25[:, 6]
rot_rs = ms25[:, 7]
rot_ps = ms25[:, 8]
rot_hs = ms25[:, 9]
bag = rosbag.Bag(sys.argv[2], 'w')
try:
for i, utime in enumerate(utimes):
timestamp = rospy.Time.from_sec(utime/1e6)
layout = MultiArrayLayout()
layout.dim = [MultiArrayDimension()]
layout.dim[0].label = "xyz"
layout.dim[0].size = 3
layout.dim[0].stride = 1
mag = Float64MultiArray()
mag.data = [mag_xs[i], mag_ys[i], mag_zs[i]]
mag.layout = layout
accel = Float64MultiArray()
accel.data = [accel_xs[i], accel_ys[i], accel_zs[i]]
accel.layout = layout
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
rot = Float64MultiArray()
rot.data = [rot_rs[i], rot_ps[i], rot_hs[i]]
rot.layout = layout_rph
bag.write('mag', mag, t=timestamp)
bag.write('accel', accel, t=timestamp)
bag.write('rot', rot, t=timestamp)
finally:
bag.close()
return 0
if __name__ == '__main__':
sys.exit(main(sys.argv))