Revision history [back]

For python it requires using tf:

import tf

tl = tf.TransformListener()
# get average twist relative to map from the most recent valid sample to
# 0.5 seconds before that
twist_tuples = tl.lookupTwist(moving_frame_id, "map",
                              rospy.Time(0.0), rospy.Duration(0.5))
linear_x = twist_tuples[0][0]

The return value isn't a Twist(), instead it's six values in two tuples which very likely are linear xyz and then angular xyz (I only pulled linear x out for my application, and it looked to be exactly that, but didn't try out angular).

Maybe it was intended to be added to tf2 but the lookupTwist is commented out: https://github.com/ros/geometry2/blob/noetic-devel/tf2_py/src/tf2_py.cpp#L350-L394

For python it requires using tf:

import tf

tl = tf.TransformListener()
# get average twist relative to map from the most recent valid sample to
# 0.5 seconds before that
twist_tuples = tl.lookupTwist(moving_frame_id, "map",
                              rospy.Time(0.0), rospy.Duration(0.5))
linear_x = twist_tuples[0][0]

The return value isn't a Twist(), instead it's six values in two tuples which very likely are linear xyz and then angular xyz (I only pulled linear x out for my application, and it looked to be exactly that, but didn't try out angular).

Maybe it was intended to be added to tf2 but the lookupTwist is commented out: https://github.com/ros/geometry2/blob/noetic-devel/tf2_py/src/tf2_py.cpp#L350-L394

For python it requires using tf:

import tf

tl = tf.TransformListener()
# get average twist relative to map from the most recent valid sample to
# 0.5 seconds before that
twist_tuples = tl.lookupTwist(moving_frame_id, "map",
                              rospy.Time(0.0), rospy.Duration(0.5))
linear_x = twist_tuples[0][0]

The return value isn't a Twist(), instead it's six values in two tuples which very likely are linear xyz and then angular xyz (I only pulled xyz- I did a test and the linear x out for my application, and it looked to be value was exactly that, but didn't try out angular).as I expected.

Maybe it was intended to be added to tf2 but the lookupTwist is commented out: https://github.com/ros/geometry2/blob/noetic-devel/tf2_py/src/tf2_py.cpp#L350-L394

Maybe this https://github.com/ros/geometry/blob/noetic-devel/tf/src/tf/listener.py#L115-L149 could be adapted into tf2 easily.

For python it requires using tf:

import tf
from geometry_msgs.msg import Twist

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

The return value isn't a Twist(), instead it's six values in two tuples which are linear xyz and then angular xyz- I did a test with (perfectly smooth) artificial data and the linear x value was exactly as I expected.

Maybe it was intended to be added to tf2 but the lookupTwist is commented out: https://github.com/ros/geometry2/blob/noetic-devel/tf2_py/src/tf2_py.cpp#L350-L394

Maybe this https://github.com/ros/geometry/blob/noetic-devel/tf/src/tf/listener.py#L115-L149 could be adapted into tf2 easily.

For python it requires using tf:

import tf
from geometry_msgs.msg import Twist
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
twist_tuples tw = tl.lookupTwist(moving_frame_id, "map",
                              rospy.Time(0.0),
rospy.Time(0.0), rospy.Duration(0.5))
twist = Twist(Vector3(tw[0][0], tw[0][1], tw[0][2],
                 rospy.Duration(0.5))
twist = Twist(twist_tuples[0], twist_tuples[1])
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- I did a test with (perfectly smooth) artificial data and the linear x value was exactly as I expected.

Maybe it was intended to be added to tf2 but the lookupTwist is commented out: https://github.com/ros/geometry2/blob/noetic-devel/tf2_py/src/tf2_py.cpp#L350-L394

Maybe this https://github.com/ros/geometry/blob/noetic-devel/tf/src/tf/listener.py#L115-L149 could be adapted into tf2 easily.

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- I did a test with (perfectly smooth) artificial data and the linear x value was exactly as I expected.

Maybe it was intended to be added to tf2 but the lookupTwist is commented out: https://github.com/ros/geometry2/blob/noetic-devel/tf2_py/src/tf2_py.cpp#L350-L394

Maybe this https://github.com/ros/geometry/blob/noetic-devel/tf/src/tf/listener.py#L115-L149 could be adapted into tf2 easily.

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],
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- I did a test with (perfectly smooth) artificial data and the linear x value was exactly as I expected.

Maybe it was intended to be added to tf2 but the lookupTwist is commented out: https://github.com/ros/geometry2/blob/noetic-devel/tf2_py/src/tf2_py.cpp#L350-L394

Maybe this https://github.com/ros/geometry/blob/noetic-devel/tf/src/tf/listener.py#L115-L149 could be adapted into tf2 easily.

# 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)

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- I did a test with (perfectly smooth) artificial data and the linear x value was exactly as I expected.xyz.

Maybe it was intended to be added to tf2 but the lookupTwist is commented out: https://github.com/ros/geometry2/blob/noetic-devel/tf2_py/src/tf2_py.cpp#L350-L394

Maybe this https://github.com/ros/geometry/blob/noetic-devel/tf/src/tf/listener.py#L115-L149 could be adapted into tf2 easily.

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/issues/43)

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/noetic-devel/tf2_py/src/tf2_py.cpp#L350-L394

Maybe this https://github.com/ros/geometry/blob/noetic-devel/tf/src/tf/listener.py#L115-L149 could be adapted into tf2 easily.