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

tf, translation between frames

asked 2014-12-09 05:16:23 -0500

psfa_fz gravatar image

Hi, I have a setup with multiple target frames. Now I want to get the position of my last frame in relation to the world frame. During execution typing

rosrun tf tf_echo /world /LAST_LINK

I receive exactly what I am looking for

- Translation: [20.632, 0.000, -3.759] - Rotation: in Quaternion [-0.574, -0.000, 0.819, 0.000] in RPY [-0.000, 1.222, 3.142]

How can I read these values in my program in order to perform further calculations from it?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2014-12-09 06:41:23 -0500

dornhege gravatar image

Read the TF tutorials.

edit flag offensive delete link more
2

answered 2014-12-12 12:02:18 -0500

Pedro_85 gravatar image

updated 2014-12-12 12:05:14 -0500

This information should be located in the /tf topic. What I used to select the header frame and child frame is:

  • subscribe to the /tf topic, your callback function will get a message of the type tf::tfMessageConstPtr let's call this message msg_in
  • msg_in will have information such as (See tfMessage.msg):
  • geometry_msgs/TransformStamped transforms
    • std_msgs/Header header
    • string child_frame_id
    • geometry_msgs/Transform transform
      • geometry_msgs/Vector3 translation
      • geometry_msgs/Quaternion rotation

You are interested in the translation and rotation components so you can access them with:

  • msg_in ->transforms[i].transform.rotation.(x, y, z, or w)
  • msg_in ->transforms[i].transform.translation(x, y, or z)

Where i is the number of the transform message. You can store them in a tf::Quaternion and a tf::Vector3 respectively. Here is a portion of an algorithm that can help you:

#include "tf/tf.h"
#include "tf/tfMessage.h"

void tfCB(const tf::tfMessageConstPtr msg)
{
  //Go through all the tf frame information and select the one you are interested in
  for(int i=0; i < msg->transforms.size();i++){
    if (msg->transforms[i].child_frame_id == "/name_of_your_child_frame"){
      cout << "Msg Size: " << msg->transforms.size() << endl;
      cout << "Header Frame: " << msg->transforms[i].header.frame_id << endl;
      cout << "Child Frame: " << msg->transforms[i].child_frame_id << endl;
      cout << "Transform: " << endl << msg->transforms[i].transform << endl;
      cout << "Vector3: " << endl << msg->transforms[i].transform.translation << endl;
      cout << "Quaternion: " << endl << msg->transforms[i].transform.rotation << endl;

      tf::Matrix3x3 Rotation;
      Rotation.setRotation(tf::Quaternion(msg->transforms[i].transform.rotation.x,
         msg->transforms[i].transform.rotation.y, msg->transforms[i].transform.rotation.z, 
         msg->transforms[i].transform.rotation.w));                                   

      tf::Vector3 traslation;
      traslation = tf::Vector3(msg->transforms[i].transform.translation.x, 
        msg->transforms[i].transform.translation.y, 
        msg->transforms[i].transform.translation.z);

      double roll, pitch, yaw;
      Rotation.getEulerYPR(yaw, pitch, roll); 

      tf::Matrix3x3 RotationYPR;
      RotationYPR.setEulerYPR(yaw,pitch,roll);
    }
  }
}

sub_tf = nh_.subscribe("/tf",100,&Marker_info::tfCB, this); //the call back function belongs to a struct called Marker_info. The idea is to show you the subscriber. You can change this to fit your code.

As you can see, you will store the rotation and translation information in a matrix and a vector that complies with the tf format. You can convert them using tools like tf::VectorTFToEigen.

Hope this helps. Good luck!

edit flag offensive delete link more

Comments

1

You should almost never directly subscribe to TF messages. This is what a TFListener is for. You don't want to look for your transformations manually.

dornhege gravatar image dornhege  ( 2014-12-12 12:59:15 -0500 )edit

Question Tools

Stats

Asked: 2014-12-09 05:16:23 -0500

Seen: 10,203 times

Last updated: Dec 12 '14