Ask Your Question
1

Convert /tf to nav_msgs/Odometry

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

TSC gravatar image

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

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 gravatar imagesai ( 2014-07-08 09:36:30 -0600 )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 gravatar imagedornhege ( 2014-07-08 10:56:50 -0600 )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 gravatar imageTSC ( 2014-07-09 06:12:30 -0600 )edit

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

dornhege gravatar imagedornhege ( 2014-07-09 06:40:41 -0600 )edit

Maybe a normal distribution for each matrix value?

TSC gravatar imageTSC ( 2014-07-09 06:58:33 -0600 )edit

That's what this hack gives you.

dornhege gravatar imagedornhege ( 2014-07-09 07:05:35 -0600 )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 gravatar imageTSC ( 2014-07-09 07:25:21 -0600 )edit

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

dornhege gravatar imagedornhege ( 2014-07-09 07:28:10 -0600 )edit

3 Answers

Sort by ยป oldest newest most voted
2

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

sai gravatar image

Have a look at poseTFToMsg

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

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 more

Comments

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

TSC gravatar imageTSC ( 2014-07-08 14:05:56 -0600 )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 gravatar imagesai ( 2014-07-08 20:56:29 -0600 )edit
1

answered 2014-07-08 08:32:15 -0600

2ROS0 gravatar image

updated 2014-07-08 10:22:50 -0600

nav_msgs/Odometry contains translation, orientation as well as twist (linear and angular velocities)

tf on the other hand only contains translation and orientation

So unless you can find a way to derive the twist...

edit flag offensive delete link more
1

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

dornhege gravatar image

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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-07-08 08:04:59 -0600

Seen: 3,896 times

Last updated: Jul 08 '14