ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

python writing tf message into bagfile serialize error

asked 2014-01-26 21:53:30 -0600

Sentinal_Bias gravatar image

updated 2014-01-28 17:19:09 -0600

ngrennan gravatar image

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2014-01-27 03:21:10 -0600

Sentinal_Bias gravatar image

updated 2014-01-27 04:58:56 -0600

        angles = tf.transformations.quaternion_from_euler(R,P,Y) 
        geo_msg.transform.rotation.x = angles[0]
        geo_msg.transform.rotation.y = angles[1]
        geo_msg.transform.rotation.z = angles[2]
        geo_msg.transform.rotation.w = angles[3]

I made the following modification to the code and it worked. It didn't work before because tf.transforms.quaternion returns an array. For some reason the parsing the array directly to transform.rotation still filled out the x,y,z,w values when I inspected them in a print statement, but resulted in a serialization error.

[update] the following code works

msgs = []
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 
        tf_msg = tfMessage()
        geo_msg = TransformStamped()
        geo_msg.header.stamp = Time(secs,nsecs) 
        geo_msg.header.seq = i
        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
        angles = tf.transformations.quaternion_from_euler(R,P,Y) 
        geo_msg.transform.rotation.x = angles[0]
        geo_msg.transform.rotation.y = angles[1]
        geo_msg.transform.rotation.z = angles[2]
        geo_msg.transform.rotation.w = angles[3]
        tf_msg.transforms.append(geo_msg)
        msgs.append(tf_msg)
return msgs

def write_bagfile(data_set_name, uwb_msgs, tf_msgs ):
    ''' Write all ROS msgs, into the bagfile. 
    '''
    bag = rosbag.Bag(data_set_name, 'w')
    # write the tf msgs into the bagfile
    for msg in tf_msgs:
        bag.write('/tf', msg, msg.transforms[0].header.stamp )
    # write the uwb msgs into the bagfile 
    for msg in uwb_msgs:
        bag.write('radar_scan', msg, msg.header.stamp)
    bag.close()
    print "Finished Bagfile IO"
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2014-01-26 21:53:30 -0600

Seen: 1,487 times

Last updated: Jan 27 '14