search

ros2 / geometry2

A set of ROS packages for keeping track of coordinate transforms.
BSD 3-Clause "New" or "Revised" License
119 stars 198 forks source link

Enable Twist interpolator python bindings #643

Open andrewbest-tri opened 9 months ago

andrewbest-tri commented 9 months ago

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.

https://github.com/ros2/geometry2/blob/1621942bc2ad1270ee0bfc1f6b7a44a5849a7b5e/tf2_py/src/tf2_py.cpp#L1061 https://github.com/ros2/geometry2/blob/1621942bc2ad1270ee0bfc1f6b7a44a5849a7b5e/tf2_py/src/tf2_py.cpp#L631

Bug report

Required Info:

Steps to reproduce issue

Expected behavior

Actual behavior

Additional information

Feature request

Feature description

Implementation considerations

ahcorde commented 9 months ago

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

ahcorde commented 9 months ago

@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 commented 9 months ago

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: https://github.com/ros2/common_interfaces/pull/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); https://github.com/ros2/geometry2/blob/1621942bc2ad1270ee0bfc1f6b7a44a5849a7b5e/tf2_ros/include/tf2_ros/buffer_interface.h#L209

ahcorde commented 9 months ago

Related PR https://github.com/ros2/geometry2/pull/646