ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
Hi Brice
I think I found a way to do this.
You must write to the bag file a tf:tfmessage instead of a geometry_msgs::TransformStamped.
Here's a piece of code that works for me (although I need to restart rviz after recording the bag file, before playing it back, in order to visualize, perhaps a problem with the timestamps, don't know)
I assume there is a tf::Transform tr, as well as a rosbag::bag bag variables
//declare a geometry msg
geometry_msgs::TransformStamped msg;
//declare a std::vector of geometry msgs std::vector< geometry_msgs::TransformStamped > vec_msg;
//declare the tf:tfmessage, this is what is actually written to the bag file
tf::tfMessage tfmsg;
//convert the tf::transform (variable tr) to a geometry msg
tf::transformStampedTFToMsg(tf::StampedTransform(tr, ros::Ti me::now(), "/world","/tf_vehicle"),msg);
//erase all value from the std::vector
vec_msg.erase(vec_msg.begin(),vec_msg.end());
//push back into the vector the geometry msg
vec_msg.push_back(msg);
//Set the tf::tfmessage transforms vector
tfmsg.set_transforms_vec(vec_msg);
//set the size (don't know if its actually needed)
tfmsg.set_transforms_size(1);
//write to the bag file
bag.write("/tf", ros::Time::now(),tfmsg);
Hope this helps
Miguel
PS: If you find why rviz needs to be restarted please let me know.