Enable Twist interpolator (#646)

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
Co-authored-by: Tully Foote <tullyfoote@intrinsic.ai>

tf2/include/tf2/buffer_core.h

@@ -46,6 +46,7 @@
 
 #include "LinearMath/Transform.h"
 #include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/velocity_stamped.hpp"
 #include "rcutils/logging_macros.h"
 #include "tf2/buffer_core_interface.h"
 #include "tf2/exceptions.h"
@@ -157,6 +158,28 @@ class BufferCore : public BufferCoreInterface
     const std::string & source_frame, const TimePoint & source_time,
     const std::string & fixed_frame) const override;
 
+  TF2_PUBLIC
+  geometry_msgs::msg::VelocityStamped lookupVelocity(
+    const std::string & tracking_frame, const std::string & observation_frame,
+    const TimePoint & time, const tf2::Duration & averaging_interval) const;
+
+  /** \brief Lookup the velocity of the moving_frame in the reference_frame
+   * \param reference_frame The frame in which to track
+   * \param moving_frame The frame to track
+   * \param time The time at which to get the velocity
+   * \param duration The period over which to average
+   * \param velocity The velocity output
+   *
+   * Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException,
+   * TransformReference::MaxDepthException
+   */
+  TF2_PUBLIC
+  geometry_msgs::msg::VelocityStamped lookupVelocity(
+    const std::string & tracking_frame, const std::string & observation_frame,
+    const std::string & reference_frame, const tf2::Vector3 & reference_point,
+    const std::string & reference_point_frame,
+    const TimePoint & time, const tf2::Duration & duration) const;
+
   /** \brief Test if a transform is possible
    * \param target_frame The frame into which to transform
    * \param source_frame The frame from which to transform

tf2/src/buffer_core.cpp

@@ -38,6 +38,8 @@
 #include <utility>
 #include <vector>
 
+#include <iostream>
+
 #include "tf2/buffer_core.h"
 #include "tf2/time_cache.h"
 #include "tf2/exceptions.h"
@@ -574,6 +576,123 @@ struct TransformAccum
   tf2::Vector3 result_vec;
 };
 
+geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity(
+  const std::string & tracking_frame, const std::string & observation_frame,
+  const TimePoint & time, const tf2::Duration & averaging_interval) const
+{
+  // ref point is origin of tracking_frame, ref_frame = obs_frame
+  return lookupVelocity(
+    tracking_frame, observation_frame, observation_frame, tf2::Vector3(
+      0, 0,
+      0), tracking_frame, time,
+    averaging_interval);
+}
+
+geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity(
+  const std::string & tracking_frame, const std::string & observation_frame,
+  const std::string & reference_frame, const tf2::Vector3 & reference_point,
+  const std::string & reference_point_frame,
+  const TimePoint & time, const tf2::Duration & averaging_interval) const
+{
+  tf2::TimePoint latest_time;
+  // TODO(anyone): This is incorrect, but better than nothing.  Really we want the latest time for
+  // any of the frames
+  getLatestCommonTime(
+    lookupFrameNumber(observation_frame),
+    lookupFrameNumber(tracking_frame),
+    latest_time,
+    0);
+
+  auto time_seconds = tf2::timeToSec(time);
+  auto averaging_interval_seconds = std::chrono::duration<double>(averaging_interval).count();
+
+  auto end_time =
+    std::min(time_seconds + averaging_interval_seconds * 0.5, tf2::timeToSec(latest_time));
+
+  auto start_time =
+    std::max(0.00001 + averaging_interval_seconds, end_time) - averaging_interval_seconds;
+  // correct for the possiblity that start time was truncated above.
+  auto corrected_averaging_interval = end_time - start_time;
+
+  tf2::Transform start, end;
+  TimePoint time_out;
+  lookupTransformImpl(
+    observation_frame, tracking_frame, tf2::timeFromSec(
+      start_time), start, time_out);
+  lookupTransformImpl(observation_frame, tracking_frame, tf2::timeFromSec(end_time), end, time_out);
+
+  auto temp = start.getBasis().inverse() * end.getBasis();
+  tf2::Quaternion quat_temp;
+  temp.getRotation(quat_temp);
+  auto o = start.getBasis() * quat_temp.getAxis();
+  auto ang = quat_temp.getAngle();
+
+  double delta_x = end.getOrigin().getX() - start.getOrigin().getX();
+  double delta_y = end.getOrigin().getY() - start.getOrigin().getY();
+  double delta_z = end.getOrigin().getZ() - start.getOrigin().getZ();
+
+  tf2::Vector3 twist_vel((delta_x) / corrected_averaging_interval,
+    (delta_y) / corrected_averaging_interval,
+    (delta_z) / corrected_averaging_interval);
+  tf2::Vector3 twist_rot = o * (ang / corrected_averaging_interval);
+
+  // correct for the position of the reference frame
+  tf2::Transform inverse;
+  lookupTransformImpl(
+    reference_frame, tracking_frame, tf2::timeFromSec(
+      time_seconds), inverse, time_out);
+  tf2::Vector3 out_rot = inverse.getBasis() * twist_rot;
+  tf2::Vector3 out_vel = inverse.getBasis() * twist_vel + inverse.getOrigin().cross(out_rot);
+
+  auto transform_point = [this](
+    const std::string & target_frame,
+    const std::string & source_frame,
+    const tf2::Vector3 & point_in,
+    double time_transform)
+    {
+      // transform point
+      tf2::Transform transform;
+      tf2::TimePoint time_out;
+      lookupTransformImpl(
+        target_frame, source_frame, tf2::timeFromSec(time_transform), transform, time_out);
+
+      tf2::Vector3 out;
+      out = transform * point_in;
+      return out;
+    };
+
+  // Rereference the twist about a new reference point
+  // Start by computing the original reference point in the reference frame:
+  tf2::Vector3 p = tf2::Vector3(0, 0, 0);
+  tf2::Vector3 rp_orig = transform_point(
+    reference_frame, tracking_frame, p, time_seconds);
+
+  tf2::Vector3 rp_desired = transform_point(
+    reference_frame, reference_point_frame, reference_point, time_seconds);
+
+  tf2::Vector3 delta = rp_desired - rp_orig;
+  out_vel = out_vel + out_rot * delta;
+
+  std::chrono::nanoseconds ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+    tf2::timeFromSec(start_time + averaging_interval_seconds * 0.5).time_since_epoch());
+  std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(
+    tf2::timeFromSec(start_time + averaging_interval_seconds * 0.5).time_since_epoch());
+  geometry_msgs::msg::VelocityStamped velocity;
+  velocity.header.stamp.sec = static_cast<int32_t>(s.count());
+  velocity.header.stamp.nanosec = static_cast<uint32_t>(ns.count() % 1000000000ull);
+  velocity.header.frame_id = reference_frame;
+  velocity.body_frame_id = tracking_frame;
+
+  velocity.velocity.linear.x = out_vel.x();
+  velocity.velocity.linear.y = out_vel.y();
+  velocity.velocity.linear.z = out_vel.z();
+  velocity.velocity.angular.x = out_rot.x();
+  velocity.velocity.angular.y = out_rot.y();
+  velocity.velocity.angular.z = out_rot.z();
+
+  return velocity;
+}
+
 geometry_msgs::msg::TransformStamped
 BufferCore::lookupTransform(
   const std::string & target_frame, const std::string & source_frame,

tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp

@@ -48,6 +48,7 @@
 #include "geometry_msgs/msg/pose_with_covariance.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
 #include "geometry_msgs/msg/polygon_stamped.hpp"
+#include "geometry_msgs/msg/velocity_stamped.hpp"
 #include "geometry_msgs/msg/wrench.hpp"
 #include "geometry_msgs/msg/wrench_stamped.hpp"
 #include "kdl/frames.hpp"
@@ -1313,6 +1314,68 @@ void fromMsg(const geometry_msgs::msg::Pose & in, tf2::Transform & out)
     tf2::Quaternion(in.orientation.x, in.orientation.y, in.orientation.z, in.orientation.w));
 }
 
+/*********************/
+/** VelocityStamped **/
+/*********************/
+
+/** \brief Apply a geometry_msgs TransformStamped to an geometry_msgs VelocityStamped type.
+ * This function is a specialization of the doTransform template defined in tf2/convert.h.
+ * \param t_in The point to transform, as a VelocityStamped message.
+ * \param t_out The transformed point, as a VelocityStamped message.
+ * \param transform The timestamped transform to apply, as a TransformStamped message.
+ */
+template<>
+inline
+void doTransform(
+  const geometry_msgs::msg::VelocityStamped & t_in,
+  geometry_msgs::msg::VelocityStamped & t_out,
+  const geometry_msgs::msg::TransformStamped & transform)
+{
+  tf2::Vector3 twist_rot(t_in.velocity.angular.x,
+    t_in.velocity.angular.y,
+    t_in.velocity.angular.z);
+  tf2::Vector3 twist_vel(t_in.velocity.linear.x,
+    t_in.velocity.linear.y,
+    t_in.velocity.linear.z);
+  tf2::Transform transform_temp;
+
+  transform_temp.setOrigin(tf2::Vector3(
+    transform.transform.translation.x,
+    transform.transform.translation.y,
+    transform.transform.translation.z));
+  transform_temp.setRotation(tf2::Quaternion(
+    transform.transform.rotation.x,
+    transform.transform.rotation.y,
+    transform.transform.rotation.z,
+    transform.transform.rotation.w));
+
+  // tf2::Transform start, end;
+  // TimePoint time_out;
+  // lookupTransformImpl(
+  //     observation_frame, tracking_frame, tf2::timeFromSec(
+  //       start_time), start, time_out);
+
+  // tf::StampedTransform transform;
+  // lookupTransform(target_frame,msg_in.header.frame_id,  msg_in.header.stamp, transform);
+
+  tf2::Vector3 out_rot = transform_temp.getBasis() * twist_rot;
+  tf2::Vector3 out_vel = transform_temp.getBasis() * twist_vel + \
+    transform_temp.getOrigin().cross(out_rot);
+
+  // geometry_msgs::TwistStamped interframe_twist;
+  // lookupVelocity(target_frame, msg_in.header.frame_id, msg_in.header.stamp,
+  //   ros::Duration(0.1), interframe_twist);
+  // \todo get rid of hard coded number
+
+  t_out.header = t_in.header;
+  t_out.velocity.linear.x = out_vel.x() + t_in.velocity.linear.x;
+  t_out.velocity.linear.y = out_vel.y() + t_in.velocity.linear.y;
+  t_out.velocity.linear.z = out_vel.z() + t_in.velocity.linear.z;
+  t_out.velocity.angular.x = out_rot.x() + t_in.velocity.angular.x;
+  t_out.velocity.angular.y = out_rot.y() + t_in.velocity.angular.y;
+  t_out.velocity.angular.z = out_rot.z() + t_in.velocity.angular.z;
+}
+
 /**********************/
 /*** WrenchStamped ****/
 /**********************/

tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp

@@ -46,6 +46,8 @@
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 
+#include <geometry_msgs/msg/velocity_stamped.hpp>
+
 std::unique_ptr<tf2_ros::Buffer> tf_buffer = nullptr;
 static const double EPS = 1e-3;
 
@@ -625,6 +627,17 @@ TEST(TfGeometry, Wrench)
   EXPECT_NEAR(res.torque.z, 5, EPS);
 }
 
+TEST(TfGeometry, Velocity)
+{
+  geometry_msgs::msg::VelocityStamped v1, res;
+  v1.header.frame_id = "world";
+  v1.body_frame_id = "base_link";
+
+  geometry_msgs::msg::TransformStamped trafo;
+
+  tf2::doTransform(v1, res, trafo);
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);

tf2_py/src/tf2_py.cpp

@@ -627,51 +627,71 @@ static PyObject * lookupTransformFullCore(PyObject * self, PyObject * args, PyOb
   // TODO(anyone): Create a converter that will actually return a python message
   return Py_BuildValue("O&", transform_converter, &transform);
 }
-/*
-static PyObject *lookupTwistCore(PyObject *self, PyObject *args, PyObject *kw)
+
+static PyObject * lookupVelocityCore(PyObject * self, PyObject * args, PyObject * kw)
 {
-  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
-  char *tracking_frame, *observation_frame;
-  builtin_interfaces::msg::Time time;
+  tf2::BufferCore * bc = ((buffer_core_t *)self)->bc;
+  char * tracking_frame, * observation_frame;
+  tf2::TimePoint time;
   tf2::Duration averaging_interval;
-  static const char *keywords[] = { "tracking_frame", "observation_frame", "time", "averaging_interval", nullptr };
+  static const char * keywords[] =
+  {"tracking_frame", "observation_frame", "time", "averaging_interval", nullptr};
 
-  if (!PyArg_ParseTupleAndKeywords(args, kw, "ssO&O&", (char**)keywords, &tracking_frame, &observation_frame, rostime_converter, &time, rosduration_converter, &averaging_interval))
+  if (!PyArg_ParseTupleAndKeywords(
+      args, kw, "ssO&O&", (char **)keywords, &tracking_frame,
+      &observation_frame, rostime_converter, &time, rosduration_converter, &averaging_interval))
+  {
     return nullptr;
-  geometry_msgs::msg::Twist twist;
-  WRAP(twist = bc->lookupTwist(tracking_frame, observation_frame, time, averaging_interval));
-
-  return Py_BuildValue("(ddd)(ddd)",
-      twist.linear.x, twist.linear.y, twist.linear.z,
-      twist.angular.x, twist.angular.y, twist.angular.z);
+  }
+  geometry_msgs::msg::VelocityStamped velocity_stamped;
+  WRAP(
+    velocity_stamped =
+    bc->lookupVelocity(tracking_frame, observation_frame, time, averaging_interval));
+
+  return Py_BuildValue(
+    "(ddd)(ddd)",
+    velocity_stamped.velocity.linear.x, velocity_stamped.velocity.linear.y,
+    velocity_stamped.velocity.linear.z,
+    velocity_stamped.velocity.angular.x, velocity_stamped.velocity.angular.y,
+    velocity_stamped.velocity.angular.z);
 }
 
-static PyObject *lookupTwistFullCore(PyObject *self, PyObject *args)
+static PyObject * lookupVelocityFullCore(PyObject * self, PyObject * args)
 {
-  tf2::BufferCore *bc = ((buffer_core_t*)self)->bc;
-  char *tracking_frame, *observation_frame, *reference_frame, *reference_point_frame;
-  builtin_interfaces::msg::Time time;
+  tf2::BufferCore * bc = ((buffer_core_t *)self)->bc;
+  char * tracking_frame, * observation_frame, * reference_frame, * reference_point_frame;
+  tf2::TimePoint time;
   tf2::Duration averaging_interval;
   double px, py, pz;
 
-  if (!PyArg_ParseTuple(args, "sss(ddd)sO&O&",
-                        &tracking_frame,
-                        &observation_frame,
-                        &reference_frame,
-                        &px, &py, &pz,
-                        &reference_point_frame,
-                        rostime_converter, &time,
-                        rosduration_converter, &averaging_interval))
+  if (!PyArg_ParseTuple(
+      args, "sss(ddd)sO&O&",
+      &tracking_frame,
+      &observation_frame,
+      &reference_frame,
+      &px, &py, &pz,
+      &reference_point_frame,
+      rostime_converter, &time,
+      rosduration_converter, &averaging_interval))
+  {
     return nullptr;
-  geometry_msgs::msg::Twist twist;
-  tf::Point pt(px, py, pz);
-  WRAP(twist = bc->lookupTwist(tracking_frame, observation_frame, reference_frame, pt, reference_point_frame, time, averaging_interval));
-
-  return Py_BuildValue("(ddd)(ddd)",
-      twist.linear.x, twist.linear.y, twist.linear.z,
-      twist.angular.x, twist.angular.y, twist.angular.z);
+  }
+  geometry_msgs::msg::VelocityStamped velocity_stamped;
+  tf2::Vector3 pt(px, py, pz);
+  WRAP(
+    velocity_stamped =
+    bc->lookupVelocity(
+      tracking_frame, observation_frame, reference_frame, pt,
+      reference_point_frame, time, averaging_interval));
+
+  return Py_BuildValue(
+    "(ddd)(ddd)",
+    velocity_stamped.velocity.linear.x, velocity_stamped.velocity.linear.y,
+    velocity_stamped.velocity.linear.z,
+    velocity_stamped.velocity.angular.x, velocity_stamped.velocity.angular.y,
+    velocity_stamped.velocity.angular.z);
 }
-*/
+
 static inline int checkTranslationType(PyObject * o)
 {
   PyTypeObject * translation_type =
@@ -1058,8 +1078,8 @@ static struct PyMethodDef buffer_core_methods[] =
     nullptr},
   {"lookup_transform_full_core", (PyCFunction)lookupTransformFullCore, METH_VARARGS | METH_KEYWORDS,
     nullptr},
-  // {"lookupTwistCore", (PyCFunction)lookupTwistCore, METH_VARARGS | METH_KEYWORDS},
-  // {"lookupTwistFullCore", lookupTwistFullCore, METH_VARARGS},
+  {"lookupVelocityCore", (PyCFunction)lookupVelocityCore, METH_VARARGS | METH_KEYWORDS, nullptr},
+  {"lookupVelocityFullCore", lookupVelocityFullCore, METH_VARARGS, nullptr},
   {nullptr, nullptr, 0, nullptr}
 };