Revision history [back]

        angles = tf.transformations.quaternion_from_euler(R,P,Y) 
        geo_msg.transform.rotation.x = angles[0]
        geo_msg.transform.rotation.x = angles[1]
        geo_msg.transform.rotation.x = angles[2]
        geo_msg.transform.rotation.x = 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

        angles = tf.transformations.quaternion_from_euler(R,P,Y) 
        geo_msg.transform.rotation.x = angles[0]
        geo_msg.transform.rotation.x geo_msg.transform.rotation.y = angles[1]
        geo_msg.transform.rotation.x geo_msg.transform.rotation.z = angles[2]
        geo_msg.transform.rotation.x 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

        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 error.

        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] these changes fixed the serialization 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"

There is still this weird issue however this code snippet does not result in tf transforms being published where I publish frames from the a bag file. The bag-file does not publish messages for long. I think tfMessages.transforms[] file and from a ros python node and they seem to interfere with each other...but that is supposed to contain different transforms, therefore if you are publishing a single transform only that vector should be only 1 unit longanother question

        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"