Convert /tf to nav_msgs/Odometry
Hi,
I want to use robot_pose_ekf to receive data from a IMU and a visual odometry source. This last one only publishes a /tf between to frames and I want to get the geometry_msgs/PoseWithCovariance from it.
Is there a proper way of converting a /tf to a nav_msgs/Odometry message?
Thanks in advance!
EDIT: Thanks all for the answers! I'm going to take a comment in a single answer. I'm using a Asus Xtion Pro Live with rgbdslam_v2 package. This last one publishes /tf between a fixed_frame and a choosen frame from us at a 10hz rate. I need this to be an odom type of message to insert it in robot_pose_ekf.
I don't really know if I'm using the right package for the purpose, but I lost a serious amount of time porting it to an ARM platform (an Odroid) and I didn't want to abandoned this one just because it doesn't give me odometry out-of-the-box. Besides, changing to, for example, to ccny-rgbd-tools will probably mean a great amount of work again to get it ported to ARM.
So can anyone point me out how can I get a proper convertion from tf to Odometry. I'm not really a hardcoder so point out some parts of code in the ccny_rgbd_tools visual_odometry.cpp would be good.
Thanks again!
The answer which was written by me gives the sample code from ccny_rgbd_tools's visual_odometry.cpp
In this case, follow the sample code. You probably want to set the covariance to something hard coded, when you put it into the EKF.
@dornhege can you please point me some code sample to hard code a good covariance matrix? I'm following the dirty fix in: http://answers.ros.org/question/66489/combine-visual-odometry-data-from-ccny_rgbd-with-encodersimu/ But it doesn't seem to be good in my case (since I will have 6DOF motion). Tkx
I'm not sure how you want to do more than a simple hack if you don't have the data.
Maybe a normal distribution for each matrix value?
That's what this hack gives you.
0.2 , 0.2 , 999999 , 999999 , 999999 , 0.2 doesn't seem Gaussian but ok. Anyway, giving that I need a 6DOF motion, give a 0.2 instead of 999999 to other values seams reasonable right?
Those are variances under the assumption that a diagonal matrix is correct.