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 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();
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!
↧