2
$\begingroup$

Note: this question was also asked in the ROS official Q&A website, but even after one week nobody has given me an answer.

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.

$\endgroup$
  • $\begingroup$ It may be easier to answer your question if you edited it to provide details of what you actually want to achieve (why you want those topics converted to the local frame), what you tried, what you saw & what you expected to see. $\endgroup$ – Mark Booth May 18 '17 at 15:52
  • $\begingroup$ This question is pretty old but, in case the OP comes back, we would need to know the message types being published on /topic1 and /topic2 to help with transforms. Knowing that messages are being published in the "/world" frame and wanting to transform them to a different frame doesn't help if we don't know /what/ we're transforming. $\endgroup$ – Joshua Whitley Aug 30 '17 at 4:35
  • $\begingroup$ @JoshuaWhitley I think you can at least give a general idea of the process, even though you don't know exactly how the messages look like. You can, for example, give more than one example of doing it by inventing structures of the messages (just for the purposes of showing how you would do it!). $\endgroup$ – nbro Aug 30 '17 at 10:45
0
$\begingroup$

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.

| improve this answer | |
$\endgroup$
0
$\begingroup$

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.

| improve this answer | |
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.