Calculating velocity from tf

edit

The standard way to get velocities from TF is with Transformer::lookupTwist(), which will return velocity as a geometry_msgs/Twist. I'm not sure how that will work with your intermittent TF broadcasting though.

update

(This doesn't exactly answer the question, the noise concerns are still valid, but possibly some searches that lead here want to do something like this, and with rospy)

I made a twist lookup from scratch, parent frame is the fixed frame (e.g. "map" or "odom" in many cases) and child frame is the moving frame:

# get the stamp of the most recent valid transform
tfs_ref = tf_buffer.lookup_transform(parent_frame, 
                                     child_frame,
                                     rospy.Time(0))
stamp = tfs_ref.header.stamp
old_stamp = stamp - rospy.Duration(duration)

tfs = tf_buffer.lookup_transform_full(child_frame, old_stamp,
                                      child_frame, stamp,
                                      parent_frame)

delta_t = 1.0 / duration
tr = tfs.transform.translation
vel_vec = Vector3(tr.x * delta_t, tr.y * delta_t, tr.z * delta_t)

rot = tfs.transform.rotation
quat = [rot.x, rot.y, rot.z, rot.w]
euler = transformations.euler_from_quaternion(quat)
rot_vec = Vector3(euler[0] * delta_t, euler[1] * delta_t, euler[2] * delta_t)
twist = Twist(vel_vec, rot_vec)

I'm not sure about the correctness, if it is producing the output any others desire, but visualizing it in rviz with mostly 2D x,y, theta motions looks reasonable (as opposed to using lookupTwist, where the twist changes as a function of angle relative to the fixed frame, when I just want the twist relative to the moving frame). Didn't look too closely at the angular rates, mostly just the linear.

(couldn't get this lookupTwist to work, even with suggested change from https://github.com/ros/geometry/issue...)

For python it requires using tf:

import tf
from geometry_msgs.msg import Twist    
from geometry_msgs.msg import Vector3

tl = tf.TransformListener()
...
# get twist relative to map calculated from the most recent
# valid sample to a sample 0.5 seconds before that
tw = tl.lookupTwist(moving_frame_id, "map",
                    rospy.Time(0.0), rospy.Duration(0.5))
twist = Twist(Vector3(tw[0][0], tw[0][1], tw[0][2]),
              Vector3(tw[1][0], tw[1][1], tw[1][2]))

The return value isn't a Twist(), instead it's six values in two tuples which are linear xyz and then angular xyz.

Maybe it was intended to be added to tf2 but the lookupTwist is commented out: https://github.com/ros/geometry2/blob...

Maybe this https://github.com/ros/geometry/blob/... could be adapted into tf2 easily.

@tfoote So this is a bit older, but I'm running into an Issue with TF2 in that I can't find a substitute function for TFs lookupTwist. Was this dropped from TF2? if so, Oops. It's really useful.

Any tips on how to calculate it myself? I've create a related question here.