Ask Your Question
3

Write a tfMessage to bag file

asked 2013-06-19 18:03:04 -0500

Alper Aydemir gravatar image

updated 2014-04-20 14:09:39 -0500

ngrennan gravatar image

Hi,

I'm trying to write a tfMessage to bag file.

I can very well write a StampedTransform using transformStampedTFToMsg, which helps generating a geometry_msgs::StampedTransform. However this results in an error in a package I'm using:

Client [/mono_odometer] wants topic /tf to have datatype/md5sum [tf/tfMessage/94810edda583a504dfda3829e70d7eec], but our version has [geometry_msgs/TransformStamped/b5764a33bfeb3588febc2682852579b0]. Dropping connection.

And this

tf::tfMessage msg; bag.write("tf", ros::Time(data->timestamp*1e-6), msg);

Does not compile (and there's no way to set a transform for msg).

What's a way of writing tfMessage to bag file while setting a transform and a timestamp?

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
1

answered 2013-06-20 22:52:38 -0500

Hi Alper,

this does the trick:

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/tfMessage.h>
#include <tf/transform_listener.h>

int main(int argc, char **argv) {
    ros::init(argc, argv, "test");
    ros::NodeHandle l_nh("~");
    std::string tf_prefix_ = tf::getPrefixParam(l_nh);

    rosbag::Bag bag;
    bag.open("test.bag", rosbag::bagmode::Write);

    geometry_msgs::TransformStamped msg;
    msg.header.frame_id = "foo";
    msg.child_frame_id = "bar";
    msg.transform.translation.x = 42.0;

    std::vector<geometry_msgs::TransformStamped> msgtf;
    msgtf.push_back(msg);

    tf::tfMessage message;
    for (std::vector<geometry_msgs::TransformStamped>::const_iterator it = msgtf.begin(); it != msgtf.end(); ++it)
    {
      message.transforms.push_back(*it);
      //Make sure to resolve anything published
      message.transforms.back().header.frame_id = tf::resolve(tf_prefix_, message.transforms.back().header.frame_id);
      message.transforms.back().child_frame_id = tf::resolve(tf_prefix_, message.transforms.back().child_frame_id);
    }
    bag.write("tf", ros::Time(1234.0), message);

    bag.close();
}

You can probably skip the ros::init, node handle and tf_prefix_ part if you don't want to support TF prefixing.

Cheers to Stockholm!

Martin

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2013-06-19 18:03:04 -0500

Seen: 842 times

Last updated: Jun 20 '13