Enable Twist interpolator python bindings

[andrewbest-tri]

The twist interpolator is commented out in python bindings. We will need it for our work and want to ensure it gets added in the python layer.

geometry2/tf2_py/src/tf2_py.cpp

Line 1061 in 1621942

// {"lookupTwistCore", (PyCFunction)lookupTwistCore, METH_VARARGS | METH_KEYWORDS},

geometry2/tf2_py/src/tf2_py.cpp

Line 631 in 1621942

static PyObject *lookupTwistCore(PyObject *self, PyObject *args, PyObject *kw)

Bug report

Required Info:

• Operating System:
• Installation type:
• Version or commit hash:
• DDS implementation:
• Client library (if applicable):

Steps to reproduce issue

Expected behavior

Actual behavior

Additional information

---

Feature request

Feature description

Implementation considerations

[ahcorde]

For reference, I found this answer in answer.ros.org

[ahcorde]

@tfoote mentioned this:

The semantics of a twist are not clearly defined in the Twist message so we can't generically transform it. And as such the method was not ported to tf2.
If you want to implement your own way to do this. It's just applying discrete differentiation. You have the algorithm right. You take the difference between position 1 and position 2 and divide by the change in time to get the "velocity". This also is not necessary as a core function as it doesn't require any extra core data. Just needs to have buffer available to query.

[tfoote]

Aside: for longevity of the link here's the R.SE copy: https://robotics.stackexchange.com/questions/85274/tf-vs-tf2-lookuptwist-vs-lookup-transform

This is pretty good timing. There's active work to reenable this by extending the Velocity message here: ros2/common_interfaces#233 so that we can have fully encoded semantics. With that extra information we can extend this logic

https://github.com/ros/geometry/blob/fe344b6c848b8239750ad7f8c7eccf86241396d3/tf/src/transform_listener.cpp#L108-L139

And add a doTransform method for tf2_geometry_msgs here: https://github.com/ros2/geometry2/blob/rolling/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp

The old implementatoin can be found here: https://github.com/ros/geometry/blob/73185068daaca224486bda84ed240fbfe86df80b/tf/src/tf.cpp#L306-L342

Note that it will need the new datatype, and also to have the extra argument for duration over which to average: https://github.com/ros/geometry/blob/73185068daaca224486bda84ed240fbfe86df80b/tf/include/tf/tf.h#L145 because it queries for the velocity value inside.

So that might need an extension to the templated T& transform(T,T, target, timeout);

geometry2/tf2_ros/include/tf2_ros/buffer_interface.h

Line 209 in 1621942

T & transform(

[ahcorde]

Related PR #646