Extract robot position from rosbag

asked 2013-05-21 21:47:02 -0500

simonw
13 ●1 ●2 ●5

updated 2014-01-28 17:16:35 -0500

ngrennan
1 ●1 ●1

I recorded tf-messages (while driving the robot) into a bagfile and wrote a ros node that should extract the positions (base_link-frame) from that bag file.

My idea was to use a tf::Transformer and feed it with all the transforms stored in the tf-messages:

rosbag::Bag bag;
bag.open("tf.bag", rosbag::bagmode::Read);
std::vector<std::string> topics;
topics.push_back("/tf");

rosbag::View view(bag, rosbag::TopicQuery(topics));

tf::Transformer transformer;

BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
    //instantiate bag message
    tf::tfMessage::ConstPtr tfPtr = m.instantiate<tf::tfMessage>();

    BOOST_FOREACH(geometry_msgs::TransformStamped const tfs, tfPtr->transforms)
    {
        tf::StampedTransform stampedTf;
        tf::transformStampedMsgToTF(tfs, stampedTf);
        //setTransform returns true!
        transformer.setTransform(stampedTf);
        ...
    }
}

The method setTransform(...) always returns true, so I thought that it works... Each time I get a transform with child_frame == /robot_1/base_link I want to get the map-base_link-transform at the time of this last transform. But the transformer returns false:

if(transformer.canTransform(baseLink, mapFrame, stampedTf.stamp_)==true){
    //lookup transform
    transformer.lookupTransform(baseLink, mapFrame, stampedTf.stamp_, temp);
}

A few additional facts: The transformer's latest common time is always = 0:

transformer.getLatestCommonTime(baseLink, mapFrame, time, NULL)

And printing all tf-messages of the bag shows that the tf-tree should be consistent (I'm using stage and used view_frames):

/map -> /robot_1/odom -> /robot_1/base_footprint -> /robot_1/base_link -> /robot_1/base_laser_link

Is something wrong with my code or idea? Any suggestions for a better solution?

Thanks in advance!

edit retag flag offensive close merge delete

Comments

Hi there, many thanks, I was looking exactly this! It worked for me! But changed: 'std::vector<std::string> topics;' -> 'std::string topic("tf");', 'rosbag::View view(bag, rosbag::TopicQuery(topics));' -> rosbag::View view(bag, rosbag::TopicQuery(topic));', and deleted 'topics.push_back("/tf");'

carlosjoserg ( 2015-05-05 09:20:44 -0500 )edit

add a comment

1

answered 2013-05-22 08:46:39 -0500

mmedvede
221 ●2 ●9 ●10

I know it would not really answer your question about whether your approach is correct. I am not familiar with rosbag API.

What I know should work is playing back the bag file and having a node with tf::TransformListener do the work of calculating your pose. Your node could look similar to this.

edit flag offensive delete link

add a comment

1

answered 2017-09-12 08:01:54 -0500

marcoesposito1988
191 ●1 ●6 ●7

Sorry for necroposting, but this might be interesting for who lands here and reads this:

I also encountered a problem and released a python package called tf_bag to use recorded tf data in a script. Hope this can be helpful.

edit flag offensive delete link

add a comment

0

answered 2018-06-01 01:02:42 -0500

StephanyB
1 ●1

Just in case if someone has the same issue:

std::string tf_msg_static     =  "/tf_static";
std::vector<std::string> topics;
topics.push_back(tf_msg_static);
rosbag::View view(bag, rosbag::TopicQuery(topics));
BOOST_FOREACH(rosbag::MessageInstance const m, view)
     {
      if (m.getTopic() == tf_msg_static || ("/" + m.getTopic() == tf_msg_static))  
      {
        tf2_msgs::TFMessage::ConstPtr tf_info = m.instantiate<tf2_msgs::TFMessage>();
        if (tf_info != NULL)
        {
          for(int i=0; i < tf_info->transforms.size();i++){
            if (tf_info->transforms[i].header.frame_id == "base_link"){
              if (tf_info->transforms[i].child_frame_id == "base_footprint"){
                Rotation_bl_bf.setRotation(tf::Quaternion(tf_info->transforms[i].transform.rotation.x, tf_info->transforms[i].transform.rotation.y, tf_info->transforms[i].transform.rotation.z,tf_info->transforms[i].transform.rotation.w));
                traslation_bl_bf = tf::Vector3(tf_info->transforms[i].transform.translation.x,tf_info->transforms[i].transform.translation.y,tf_info->transforms[i].transform.translation.z);
                Matrix_bl_bf << Rotation_bl_bf.getRow(0)[0],Rotation_bl_bf.getRow(0)[1] ,Rotation_bl_bf.getRow(0)[2],traslation_bl_bf.getX(),
                                Rotation_bl_bf.getRow(1)[0],Rotation_bl_bf.getRow(1)[1] ,Rotation_bl_bf.getRow(1)[2],traslation_bl_bf.getY(),
                                Rotation_bl_bf.getRow(2)[0],Rotation_bl_bf.getRow(2)[1] ,Rotation_bl_bf.getRow(2)[2],traslation_bl_bf.getZ(),
                                0,0,0,1;
              }
              if (tf_info->transforms[i].child_frame_id == "velodyne_front_link"){
                Rotation_bl_vl.setRotation(tf::Quaternion(tf_info->transforms[i].transform.rotation.x, tf_info->transforms[i].transform.rotation.y, tf_info->transforms[i].transform.rotation.z,tf_info->transforms[i].transform.rotation.w));
                traslation_bl_vl = tf::Vector3(tf_info->transforms[i].transform.translation.x,tf_info->transforms[i].transform.translation.y,tf_info->transforms[i].transform.translation.z);
                Matrix_bl_vl << Rotation_bl_vl.getRow(0)[0],Rotation_bl_vl.getRow(0)[1] ,Rotation_bl_vl.getRow(0)[2],traslation_bl_vl.getX(),
                                Rotation_bl_vl.getRow(1)[0],Rotation_bl_vl.getRow(1)[1] ,Rotation_bl_vl.getRow(1)[2],traslation_bl_vl.getY(),
                                Rotation_bl_vl.getRow(2)[0],Rotation_bl_vl.getRow(2)[1] ,Rotation_bl_vl.getRow(2)[2],traslation_bl_vl.getZ(),
                                0,0,0,1;
              }
              Matrix_bf_vl = Matrix_bl_bf.pow(-1) * Matrix_bl_vl;
              }
            }
          }
        }