Transform timestamped messages recorded in a bag file using tf messages

Question

I have a .bag which contains recorded messages on topics /topic1 and /topic2. The messages have /world as frame_id, so both of the messages associated with these topics are stamped, i.e. they have a header.

The same .bag file also contains recorded messages on the topic /tf (of type tf2_msgs/TFMessage). These transform messages have the frame_id set to /world and the child_frame_id set to the local frames associated with the IMUs from which the messages are being sent over respectively topic /topic1 and /topic2.

Now, I need the messages sent over the /topic1 and /topic2 to be converted to their corresponding local frame (i.e. the child frame or child_frame_id ) from the (fixed) frame /world. Since both the /tf messages and the messages of the topics /topic1 and /topic2 are stamped, I thought we could do this without much trouble, but I'm not sure since I'm very new to ROS.

I've looked around for various solutions, but I didn't find an exact solution for my problem, maybe because I didn't recognize it as such, given my limited knowledge of ROS, as I said.

I would appreciate a step by step description of the approach and, if you don't want to write a full solution with code (preferably in Python), at least point me to the similar examples. Please, do not suggest me to read the tutorials of /tf, I've partially done it, and it didn't help much.

Answer 1

As far I understand, you have a topic /topic and an associated transformation on the tf topic. You then want to move the pose according to the tf transformation.

TF provides you with a tree representing all the transformations and the changes in time. It's really useful for easily tracking the position of each sensor on the robot for example. By knowing the transformation of each sensor to the base of the robot, you only need to change the transformation between the world and the base of the robot to know the position of all sensors in space.

If you want to transform /topic to its actual pose in tf, you'll need first to read the transformation from the tf topic from frame_id to child_frame_id. Then apply this transformation to the pose in topic at the same time. An efficient way to get messages at the same time is to use message filter. I'm sure you'll find plenty of example to do just that, for example here.

Answer 2

In general terms, transforms between coordinate frames in ROS are achieved through the tf API. Strictly speaking we should actually use tf2, but since its documentation still has some catch up to do you might want to stick to tf for the time being.

The Python tf API makes it easy to convert geometric messages between coordinate frames. Following the scenario you outlined, converting IMU data from global to local coordinate frames could be done as below:

import rospy
import tf

import sensor_msgs.msg
import geometry_msgs.msg

class IMUListener(object):
    def __init__(self):
        self.tf_listener = tf.TransformListener()
        self.global_frame = rospy.get_param('~global_frame', 'world')
        self.subscriber = rospy.Subscriber('imu', sensor_msgs.msg.Imu, self.callback, queue_size=1)

    def callback(self, imu):
        global_frame = self.global_frame
        local_frame = imu.header.frame_id

        if not (self.tf.frameExists(global_frame) and self.tf.frameExists(local_frame)):
            rospy.logerr('Could not transform between frames "%s" and "%s"' % (global_frame, local_frame))
            return

        # Build a stamped quaternion message containing the IMU orientation readings,
        # timestamp and source reference frame (presumed to be the global reference frame).
        reading = geometry_msgs.msg.QuaternionStamped()
        reading.header.seq = imu.header.seq
        reading.header.stamp = imu.header.stamp
        reading.header.frame_id = global_frame
        reading.quaternion = imu.orientation

        # Convert the input quaternion from its source frame (taken from header.frame_id)
        # to the given target frame, using the transform available at time header.stamp.
        transformed = self.tf_listener.transformQuaternion(local_frame, reading)

        print transformed

This code assumes that your problem is that even though IMU messages are assigned the sensor's frame id, the actual readings are relative to the global frame, and therefore you need a way to convert them to the right reference. It uses a tf.TransformListener object to listen to messages in the /tf topic and extract time-stamped transforms between coordinate frames. The method transformQuaternion() transforms the input QuaternionStamped message from its assigned header.frame_id reference frame to the target frame, using the transform between source and target frames that was in effect at time header.stamp.