Convert /tf to nav_msgs/Odometry

asked 2014-07-08 08:04:59 -0500

TSC
180 ●16 ●21 ●29

updated 2014-07-08 09:11:18 -0500

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!

edit retag flag offensive close merge delete

Comments

The answer which was written by me gives the sample code from ccny_rgbd_tools's visual_odometry.cpp

sai ( 2014-07-08 09:36:30 -0500 )edit

1

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 ( 2014-07-08 10:56:50 -0500 )edit

@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

TSC ( 2014-07-09 06:12:30 -0500 )edit

I'm not sure how you want to do more than a simple hack if you don't have the data.

dornhege ( 2014-07-09 06:40:41 -0500 )edit

Maybe a normal distribution for each matrix value?

TSC ( 2014-07-09 06:58:33 -0500 )edit

That's what this hack gives you.

dornhege ( 2014-07-09 07:05:35 -0500 )edit

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?

TSC ( 2014-07-09 07:25:21 -0500 )edit

Those are variances under the assumption that a diagonal matrix is correct.

dornhege ( 2014-07-09 07:28:10 -0500 )edit

add a comment

2

answered 2014-07-08 08:23:55 -0500

sai
1925 ●58 ●75 ●86

Have a look at poseTFToMsg

http://mirror.umd.edu/roswiki/doc/dia...

Sample code block from here https://github.com/ccny-ros-pkg/ccny_...

This code below converts a TF msg to a odometry msg, you can find example for pose msg also in the link above

             void VisualOdometry::publishOdom(const std_msgs::Header& header)
          {
                OdomMsg odom;
                odom.header.stamp = header.stamp;
                odom.header.frame_id = fixed_frame_;
                 tf::poseTFToMsg(f2b_, odom.pose.pose);
                  odom_publisher_.publish(odom);
          }

edit flag offensive delete link

Comments

I will check this. Probably have to adapt graph_manager.cpp in rgbdslam src folder to get this kind of message. Thanks!

TSC ( 2014-07-08 14:05:56 -0500 )edit

I do not think there is need to look at graph_manager.cpp. In the above visual_odometry.cpp, you can find another function called 'publishTf' that actually converts the estimated transformation to a TF and also to odom msg, pose msg etc. I think completely going through the visual_odometry.cpp and its header file where `f2b_` is defined should help you a lot.

sai ( 2014-07-08 20:56:29 -0500 )edit

add a comment

1

answered 2014-07-08 08:28:06 -0500

dornhege

31395 ●134 ●284 ●498

Proper way: No

Why: Odometry contains Twist and Covariances. Both are not present in /tf. You could derivate tf information to get Twists and maybe somehow give covariances, but I wouldn't consider that proper. Ideally ones goes the other way and produces /tf from Odometry.

Depending on what you want to do with the data, Just putting pose information into the odometry might be sufficient for the application that needs an Odometry message.