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.
