Write a tfMessage to bag file

edit

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