tf, translation between frames

edit

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!