python writing tf message into bagfile serialize error
Hi
I am trying to write tf messages into a bagfile but I am getting this error message.
File "converter.py", line 131, in write_bagfile
bag.write('tf',tf_msgs, tf_msgs.transforms[0].header.stamp )
File "/opt/ros/groovy/lib/python2.7/dist-packages/rosbag/bag.py", line 375, in write
msg.serialize(self._buffer)
File "/opt/ros/groovy/lib/python2.7/dist-packages/tf/msg/_tfMessage.py", line 134, in serialize
buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
Here my current code
def read_nav_csv(fname, origin):
''' Reads the nav.csv , converts into a local coordinate frame and returns tf msgs '''
tf_msg = tfMessage()
with open(fname, 'r') as f:
for i, line in enumerate(f):
words = line.split(',')
time = words[0]
[secs, nsecs] = map(int, time.split('.'))
x = float(words[1])
y = float(words[2])
z = float(words[3])
R = float(words[4])
P = float(words[5])
Y = float(words[6])
# convert to local coordinate frame, located at the origin
x = x - origin[0]
y = y - origin[1]
z = z - origin[2]
# create TF msg
geo_msg = TransformStamped()
geo_msg.header.stamp = Time(secs,nsecs)
geo_msg.header.frame_id = "/map"
geo_msg.child_frame_id = "/body"
geo_msg.transform.translation.x = x
geo_msg.transform.translation.y = y
geo_msg.transform.translation.z = z
geo_msg.transform.rotation = tf.transformations.quaternion_from_euler(R,P,Y) # ax, ay, az
tf_msg.transforms.append(geo_msg)
return tf_msg
Then I write the msg into the bagfile like this
# tf_msgs <--- tf_msg from above function call
bag = rosbag.Bag(data_set_name, 'w')
# write the tf msgs into the bagfile
bag.write('tf',tf_msgs, tf_msgs.transforms[0].header.stamp )
I am not sure what to make of this error, I have printed the geometry msgs to make sure all the attributes have been set like x,y,z and rotation.