0
$\begingroup$

Rosanswers logo

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?


Originally posted by psfa_fz on ROS Answers with karma: 36 on 2014-12-09

Post score: 1

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

Read the TF tutorials.


Originally posted by dornhege with karma: 31395 on 2014-12-09

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$
0
$\begingroup$

Rosanswers logo

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!


Originally posted by Pedro_85 with karma: 182 on 2014-12-12

This answer was NOT ACCEPTED on the original site

Post score: 2


Original comments

Comment by dornhege on 2014-12-12:
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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.