Add Tf2 versions of lookupTransform, lookupVelocity, setTransform

tf2/include/tf2/buffer_core.h

@@ -124,11 +124,28 @@ class BufferCore : public BufferCoreInterface
 
   /*********** Accessors *************/
 
+  /**
+   * \brief Get the transform between two frames by frame ID.
+   * \param target_frame The frame to which data should be transformed.
+   * \param source_frame The frame where the data originated.
+   * \param time The time at which the value of the transform is desired (0 will get the latest).
+   * \return The transform between the frames.
+   *
+   * Possible exceptions tf2::LookupException, tf2::ConnectivityException,
+   * tf2::ExtrapolationException, tf2::InvalidArgumentException
+   */
+  TF2_PUBLIC
+  tf2::Stamped<tf2::Transform>
+  lookupTransformTf2(
+    const std::string & target_frame,
+    const std::string & source_frame,
+    const tf2::TimePoint & time) const override;
+
   /** \brief Get the transform between two frames by frame ID.
    * \param target_frame The frame to which data should be transformed
    * \param source_frame The frame where the data originated
    * \param time The time at which the value of the transform is desired. (0 will get the latest)
-   * \return The transform between the frames
+   * \return The transform between the frames as a ROS type.
    *
    * Possible exceptions tf2::LookupException, tf2::ConnectivityException,
    * tf2::ExtrapolationException, tf2::InvalidArgumentException
@@ -139,13 +156,35 @@ class BufferCore : public BufferCoreInterface
     const std::string & target_frame, const std::string & source_frame,
     const TimePoint & time) const override;
 
+  /**
+   * \brief Get the transform between two frames by frame ID assuming fixed frame.
+   * \param target_frame The frame to which data should be transformed.
+   * \param target_time The time to which the data should be transformed (0 will get the latest).
+   * \param source_frame The frame where the data originated.
+   * \param source_time The time at which the source_frame should be evaluated
+   *   (0 will get the latest).
+   * \param fixed_frame The frame in which to assume the transform is constant in time.
+   * \return The transform between the frames.
+   *
+   * Possible exceptions tf2::LookupException, tf2::ConnectivityException,
+   * tf2::ExtrapolationException, tf2::InvalidArgumentException
+   */
+  TF2_PUBLIC
+  tf2::Stamped<tf2::Transform>
+  lookupTransformTf2(
+    const std::string & target_frame,
+    const tf2::TimePoint & target_time,
+    const std::string & source_frame,
+    const tf2::TimePoint & source_time,
+    const std::string & fixed_frame) const override;
+
   /** \brief Get the transform between two frames by frame ID assuming fixed frame.
    * \param target_frame The frame to which data should be transformed
    * \param target_time The time to which the data should be transformed. (0 will get the latest)
    * \param source_frame The frame where the data originated
    * \param source_time The time at which the source_frame should be evaluated. (0 will get the latest)
    * \param fixed_frame The frame in which to assume the transform is constant in time.
-   * \return The transform between the frames
+   * \return The transform between the frames as a ROS type.
    *
    * Possible exceptions tf2::LookupException, tf2::ConnectivityException,
    * tf2::ExtrapolationException, tf2::InvalidArgumentException
@@ -158,6 +197,11 @@ class BufferCore : public BufferCoreInterface
     const std::string & source_frame, const TimePoint & source_time,
     const std::string & fixed_frame) const override;
 
+  TF2_PUBLIC
+  tf2::Stamped<std::pair<tf2::Vector3, tf2::Vector3>> lookupVelocityTf2(
+    const std::string & tracking_frame, const std::string & observation_frame,
+    const TimePoint & time, const tf2::Duration & averaging_interval) const;
+
   TF2_PUBLIC
   geometry_msgs::msg::VelocityStamped lookupVelocity(
     const std::string & tracking_frame, const std::string & observation_frame,
@@ -174,6 +218,23 @@ class BufferCore : public BufferCoreInterface
    * TransformReference::MaxDepthException
    */
   TF2_PUBLIC
+  tf2::Stamped<std::pair<tf2::Vector3, tf2::Vector3>> lookupVelocityTf2(
+    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 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 as a ROS type
+   *
+   * 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,

tf2/include/tf2/buffer_core_interface.h

@@ -34,6 +34,7 @@
 #include "geometry_msgs/msg/transform_stamped.hpp"
 
 #include "tf2/time.h"
+#include "tf2/transform_datatypes.h"
 #include "tf2/visibility_control.h"
 
 namespace tf2
@@ -67,6 +68,20 @@ class BufferCoreInterface
    * \return The transform between the frames.
    */
   TF2_PUBLIC
+  virtual tf2::Stamped<tf2::Transform>
+  lookupTransformTf2(
+    const std::string & target_frame,
+    const std::string & source_frame,
+    const tf2::TimePoint & time) const = 0;
+
+  /**
+   * \brief Get the transform between two frames by frame ID.
+   * \param target_frame The frame to which data should be transformed.
+   * \param source_frame The frame where the data originated.
+   * \param time The time at which the value of the transform is desired (0 will get the latest).
+   * \return The transform between the frames as a ROS type.
+   */
+  TF2_PUBLIC
   virtual geometry_msgs::msg::TransformStamped
   lookupTransform(
     const std::string & target_frame,
@@ -84,6 +99,25 @@ class BufferCoreInterface
    * \return The transform between the frames.
    */
   TF2_PUBLIC
+  virtual tf2::Stamped<tf2::Transform>
+  lookupTransformTf2(
+    const std::string & target_frame,
+    const tf2::TimePoint & target_time,
+    const std::string & source_frame,
+    const tf2::TimePoint & source_time,
+    const std::string & fixed_frame) const = 0;
+
+  /**
+   * \brief Get the transform between two frames by frame ID assuming fixed frame.
+   * \param target_frame The frame to which data should be transformed.
+   * \param target_time The time to which the data should be transformed (0 will get the latest).
+   * \param source_frame The frame where the data originated.
+   * \param source_time The time at which the source_frame should be evaluated
+   *   (0 will get the latest).
+   * \param fixed_frame The frame in which to assume the transform is constant in time.
+   * \return The transform between the frames as a ROS type.
+   */
+  TF2_PUBLIC
   virtual geometry_msgs::msg::TransformStamped
   lookupTransform(
     const std::string & target_frame,

tf2/src/buffer_core.cpp

@@ -98,6 +98,29 @@ void fillOrWarnMessageForInvalidFrame(
   }
 }
 
+/*
+ * remove when geometry_msgs dependency is broken, tf2_geometry_msgs provides a better implementation
+ */
+geometry_msgs::msg::TransformStamped toMsg(const tf2::Stamped<tf2::Transform> & stamped_transform)
+{
+  geometry_msgs::msg::TransformStamped msg;
+  msg.transform.translation.x = stamped_transform.getOrigin().x();
+  msg.transform.translation.y = stamped_transform.getOrigin().y();
+  msg.transform.translation.z = stamped_transform.getOrigin().z();
+  msg.transform.rotation.x = stamped_transform.getRotation().x();
+  msg.transform.rotation.y = stamped_transform.getRotation().y();
+  msg.transform.rotation.z = stamped_transform.getRotation().z();
+  msg.transform.rotation.w = stamped_transform.getRotation().w();
+  std::chrono::nanoseconds ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+    stamped_transform.stamp_.time_since_epoch());
+  std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(
+    stamped_transform.stamp_.time_since_epoch());
+  msg.header.stamp.sec = static_cast<int32_t>(s.count());
+  msg.header.stamp.nanosec = static_cast<uint32_t>(ns.count() % 1000000000ull);
+  msg.header.frame_id = stamped_transform.frame_id_;
+  return msg;
+}
+
 }  // anonymous namespace
 
 CompactFrameID BufferCore::validateFrameId(
@@ -576,6 +599,18 @@ struct TransformAccum
   tf2::Vector3 result_vec;
 };
 
+tf2::Stamped<std::pair<tf2::Vector3, tf2::Vector3>> BufferCore::lookupVelocityTf2(
+  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 lookupVelocityTf2(
+    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 TimePoint & time, const tf2::Duration & averaging_interval) const
@@ -588,7 +623,7 @@ geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity(
     averaging_interval);
 }
 
-geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity(
+tf2::Stamped<std::pair<tf2::Vector3, tf2::Vector3>> BufferCore::lookupVelocityTf2(
   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,
@@ -673,80 +708,103 @@ geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity(
   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;
+  const tf2::TimePoint out_time = tf2::timeFromSec(start_time + averaging_interval_seconds * 0.5);
+
+  return tf2::Stamped<std::pair<tf2::Vector3, tf2::Vector3>>(
+    {out_vel, out_rot}, out_time,
+    reference_frame);
+}
+
+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
+{
+  const tf2::Stamped<std::pair<tf2::Vector3, tf2::Vector3>> stamped_velocity = lookupVelocityTf2(
+    tracking_frame, observation_frame, reference_frame,
+    reference_point, reference_point_frame, time, averaging_interval);
+
+  geometry_msgs::msg::VelocityStamped msg;
+  const std::chrono::nanoseconds ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
+    stamped_velocity.stamp_.time_since_epoch());
+  const std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(
+    stamped_velocity.stamp_.time_since_epoch());
+  msg.header.stamp.sec = static_cast<int32_t>(s.count());
+  msg.header.stamp.nanosec = static_cast<uint32_t>(ns.count() % 1000000000ull);
+  msg.header.frame_id = reference_frame;
+  msg.body_frame_id = tracking_frame;
+
+  msg.velocity.linear.x = stamped_velocity.first.x();
+  msg.velocity.linear.y = stamped_velocity.first.y();
+  msg.velocity.linear.z = stamped_velocity.first.z();
+  msg.velocity.angular.x = stamped_velocity.second.x();
+  msg.velocity.angular.y = stamped_velocity.second.y();
+  msg.velocity.angular.z = stamped_velocity.second.z();
+
+  return msg;
 }
 
+tf2::Stamped<tf2::Transform>
+BufferCore::lookupTransformTf2(
+  const std::string & target_frame, const std::string & source_frame,
+  const TimePoint & time) const
+{
+  tf2::Stamped<tf2::Transform> stamped_transform;
+  lookupTransformImpl(
+    target_frame, source_frame, time, stamped_transform,
+    stamped_transform.stamp_);
+  stamped_transform.frame_id_ = target_frame;
+  return stamped_transform;
+}
+
+
 geometry_msgs::msg::TransformStamped
 BufferCore::lookupTransform(
   const std::string & target_frame, const std::string & source_frame,
   const TimePoint & time) const
 {
-  tf2::Transform transform;
-  TimePoint time_out;
-  lookupTransformImpl(target_frame, source_frame, time, transform, time_out);
-  geometry_msgs::msg::TransformStamped msg;
-  msg.transform.translation.x = transform.getOrigin().x();
-  msg.transform.translation.y = transform.getOrigin().y();
-  msg.transform.translation.z = transform.getOrigin().z();
-  msg.transform.rotation.x = transform.getRotation().x();
-  msg.transform.rotation.y = transform.getRotation().y();
-  msg.transform.rotation.z = transform.getRotation().z();
-  msg.transform.rotation.w = transform.getRotation().w();
-  std::chrono::nanoseconds ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
-    time_out.time_since_epoch());
-  std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(
-    time_out.time_since_epoch());
-  msg.header.stamp.sec = static_cast<int32_t>(s.count());
-  msg.header.stamp.nanosec = static_cast<uint32_t>(ns.count() % 1000000000ull);
-  msg.header.frame_id = target_frame;
+  const tf2::Stamped<tf2::Transform> stamped_transform = lookupTransformTf2(
+    target_frame,
+    source_frame, time);
+
+  geometry_msgs::msg::TransformStamped msg = toMsg(stamped_transform);
   msg.child_frame_id = source_frame;
 
   return msg;
 }
 
-geometry_msgs::msg::TransformStamped
-BufferCore::lookupTransform(
+
+tf2::Stamped<tf2::Transform>
+BufferCore::lookupTransformTf2(
   const std::string & target_frame, const TimePoint & target_time,
   const std::string & source_frame, const TimePoint & source_time,
   const std::string & fixed_frame) const
 {
-  tf2::Transform transform;
-  TimePoint time_out;
+  tf2::Stamped<tf2::Transform> stamped_transform;
+
   lookupTransformImpl(
     target_frame, target_time, source_frame, source_time,
-    fixed_frame, transform, time_out);
-  geometry_msgs::msg::TransformStamped msg;
-  msg.transform.translation.x = transform.getOrigin().x();
-  msg.transform.translation.y = transform.getOrigin().y();
-  msg.transform.translation.z = transform.getOrigin().z();
-  msg.transform.rotation.x = transform.getRotation().x();
-  msg.transform.rotation.y = transform.getRotation().y();
-  msg.transform.rotation.z = transform.getRotation().z();
-  msg.transform.rotation.w = transform.getRotation().w();
-  std::chrono::nanoseconds ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
-    time_out.time_since_epoch());
-  std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(
-    time_out.time_since_epoch());
-  msg.header.stamp.sec = static_cast<int32_t>(s.count());
-  msg.header.stamp.nanosec = static_cast<uint32_t>(ns.count() % 1000000000ull);
-  msg.header.frame_id = target_frame;
+    fixed_frame, stamped_transform, stamped_transform.stamp_);
+
+  return stamped_transform;
+}
+
+
+geometry_msgs::msg::TransformStamped
+BufferCore::lookupTransform(
+  const std::string & target_frame, const TimePoint & target_time,
+  const std::string & source_frame, const TimePoint & source_time,
+  const std::string & fixed_frame) const
+{
+  const tf2::Stamped<tf2::Transform> stamped_transform = lookupTransformTf2(
+    target_frame,
+    target_time,
+    source_frame,
+    source_time,
+    fixed_frame);
+
+  geometry_msgs::msg::TransformStamped msg = toMsg(stamped_transform);
   msg.child_frame_id = source_frame;
 
   return msg;