From eee73eba95caf5987f5703849f4250451cbc0b5f Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Mon, 11 Nov 2024 14:53:41 -0800 Subject: [PATCH 1/5] Add generic interface to tf2 Signed-off-by: kyle-basis --- tf2/include/tf2/buffer_core_interface.h | 33 +++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/tf2/include/tf2/buffer_core_interface.h b/tf2/include/tf2/buffer_core_interface.h index f68512a6c..3ec265944 100644 --- a/tf2/include/tf2/buffer_core_interface.h +++ b/tf2/include/tf2/buffer_core_interface.h @@ -67,6 +67,20 @@ class BufferCoreInterface * \return The transform between the frames. */ TF2_PUBLIC + virtual 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 +98,25 @@ class BufferCoreInterface * \return The transform between the frames. */ TF2_PUBLIC + virtual 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, From 62b005d96cc397cf85b2cb863be69aca9d1a0afe Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Mon, 11 Nov 2024 23:47:28 -0800 Subject: [PATCH 2/5] add *Tf2 versions of functions --- tf2/include/tf2/buffer_core.h | 65 +++++++++- tf2/include/tf2/buffer_core_interface.h | 5 +- tf2/src/buffer_core.cpp | 160 +++++++++++++++--------- 3 files changed, 169 insertions(+), 61 deletions(-) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index 0c949e944..59070360e 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/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 + virtual tf2::Stamped + 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 + virtual tf2::Stamped + 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,11 +197,33 @@ class BufferCore : public BufferCoreInterface const std::string & source_frame, const TimePoint & source_time, const std::string & fixed_frame) const override; + TF2_PUBLIC + tf2::Stamped> 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, 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 + tf2::Stamped> 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 diff --git a/tf2/include/tf2/buffer_core_interface.h b/tf2/include/tf2/buffer_core_interface.h index 3ec265944..de131bca3 100644 --- a/tf2/include/tf2/buffer_core_interface.h +++ b/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,7 +68,7 @@ class BufferCoreInterface * \return The transform between the frames. */ TF2_PUBLIC - virtual tf2::Transform + virtual tf2::Stamped lookupTransformTf2( const std::string & target_frame, const std::string & source_frame, @@ -98,7 +99,7 @@ class BufferCoreInterface * \return The transform between the frames. */ TF2_PUBLIC - virtual tf2::Transform + virtual tf2::Stamped lookupTransformTf2( const std::string & target_frame, const tf2::TimePoint & target_time, diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 97fa96bca..d0da61532 100644 --- a/tf2/src/buffer_core.cpp +++ b/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 & 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( + stamped_transform.stamp_.time_since_epoch()); + std::chrono::seconds s = std::chrono::duration_cast( + stamped_transform.stamp_.time_since_epoch()); + msg.header.stamp.sec = static_cast(s.count()); + msg.header.stamp.nanosec = static_cast(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> 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> 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,91 @@ 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( - tf2::timeFromSec(start_time + averaging_interval_seconds * 0.5).time_since_epoch()); - std::chrono::seconds s = std::chrono::duration_cast( - tf2::timeFromSec(start_time + averaging_interval_seconds * 0.5).time_since_epoch()); - geometry_msgs::msg::VelocityStamped velocity; - velocity.header.stamp.sec = static_cast(s.count()); - velocity.header.stamp.nanosec = static_cast(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>({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> 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( + stamped_velocity.stamp_.time_since_epoch()); + const std::chrono::seconds s = std::chrono::duration_cast( + stamped_velocity.stamp_.time_since_epoch()); + msg.header.stamp.sec = static_cast(s.count()); + msg.header.stamp.nanosec = static_cast(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 +BufferCore::lookupTransformTf2( + const std::string & target_frame, const std::string & source_frame, + const TimePoint & time) const +{ + tf2::Stamped 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( - time_out.time_since_epoch()); - std::chrono::seconds s = std::chrono::duration_cast( - time_out.time_since_epoch()); - msg.header.stamp.sec = static_cast(s.count()); - msg.header.stamp.nanosec = static_cast(ns.count() % 1000000000ull); - msg.header.frame_id = target_frame; + const tf2::Stamped 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 +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 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( - time_out.time_since_epoch()); - std::chrono::seconds s = std::chrono::duration_cast( - time_out.time_since_epoch()); - msg.header.stamp.sec = static_cast(s.count()); - msg.header.stamp.nanosec = static_cast(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 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; From 13104aa58515c110a67b9a95223ece6214a5fbab Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 13 Nov 2024 14:06:23 -0800 Subject: [PATCH 3/5] uncrustify --- tf2/src/buffer_core.cpp | 22 +++++++++++++++++----- tf2/test/cache_benchmark.cpp | 16 ++++++++-------- tf2/test/cache_unittest.cpp | 4 ++-- 3 files changed, 27 insertions(+), 15 deletions(-) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index d0da61532..bfe9b2332 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -710,7 +710,9 @@ tf2::Stamped> BufferCore::lookupVelocityTf const tf2::TimePoint out_time = tf2::timeFromSec(start_time + averaging_interval_seconds * 0.5); - return tf2::Stamped>({out_vel, out_rot}, out_time, reference_frame); + return tf2::Stamped>( + {out_vel, out_rot}, out_time, + reference_frame); } geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity( @@ -719,7 +721,8 @@ geometry_msgs::msg::VelocityStamped BufferCore::lookupVelocity( const std::string & reference_point_frame, const TimePoint & time, const tf2::Duration & averaging_interval) const { - const tf2::Stamped> stamped_velocity = lookupVelocityTf2(tracking_frame, observation_frame, reference_frame, + const tf2::Stamped> stamped_velocity = lookupVelocityTf2( + tracking_frame, observation_frame, reference_frame, reference_point, reference_point_frame, time, averaging_interval); geometry_msgs::msg::VelocityStamped msg; @@ -748,7 +751,9 @@ BufferCore::lookupTransformTf2( const TimePoint & time) const { tf2::Stamped stamped_transform; - lookupTransformImpl(target_frame, source_frame, time, stamped_transform, stamped_transform.stamp_); + lookupTransformImpl( + target_frame, source_frame, time, stamped_transform, + stamped_transform.stamp_); stamped_transform.frame_id_ = target_frame; return stamped_transform; } @@ -759,7 +764,9 @@ BufferCore::lookupTransform( const std::string & target_frame, const std::string & source_frame, const TimePoint & time) const { - const tf2::Stamped stamped_transform = lookupTransformTf2(target_frame, source_frame, time); + const tf2::Stamped stamped_transform = lookupTransformTf2( + target_frame, + source_frame, time); geometry_msgs::msg::TransformStamped msg = toMsg(stamped_transform); msg.child_frame_id = source_frame; @@ -790,7 +797,12 @@ BufferCore::lookupTransform( const std::string & source_frame, const TimePoint & source_time, const std::string & fixed_frame) const { - const tf2::Stamped stamped_transform = lookupTransformTf2(target_frame, target_time, source_frame, source_time, fixed_frame); + const tf2::Stamped 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; diff --git a/tf2/test/cache_benchmark.cpp b/tf2/test/cache_benchmark.cpp index af7241f08..5ad8b3189 100644 --- a/tf2/test/cache_benchmark.cpp +++ b/tf2/test/cache_benchmark.cpp @@ -74,10 +74,10 @@ static void benchmark_insertion(benchmark::State & state) // First, fill the cache with max storage amount (the limit). tf2::TimeCache fill_cache(max_storage_time); const auto [fill_timestamp, fill_timestep] = insert_data( - fill_cache, - tf2::TimePointZero, - 0, - tf2::TimePointZero + max_storage_time); + fill_cache, + tf2::TimePointZero, + 0, + tf2::TimePointZero + max_storage_time); // Now profile adding new data to the copied cache. const tf2::TimePoint target_timestamp = fill_timestamp + max_storage_time; @@ -88,10 +88,10 @@ static void benchmark_insertion(benchmark::State & state) state.ResumeTiming(); insert_data( - cache, - fill_timestamp, - fill_timestep, - target_timestamp); + cache, + fill_timestamp, + fill_timestep, + target_timestamp); } } diff --git a/tf2/test/cache_unittest.cpp b/tf2/test/cache_unittest.cpp index e00aa0c10..91069f7a3 100644 --- a/tf2/test/cache_unittest.cpp +++ b/tf2/test/cache_unittest.cpp @@ -137,8 +137,8 @@ TEST(TimeCache, GetAllItems) // Note that the difference between the oldest and newest timestamp is exactly equal // to the max storage duration. EXPECT_EQ( - cache.getLatestTimestamp() - cache.getOldestTimestamp(), - max_storage_time); + cache.getLatestTimestamp() - cache.getOldestTimestamp(), + max_storage_time); // Expect that storage is descending. const std::list & storage_expected{ From ba44b56535770c1b86ecec4a837c0d8f0bc8157e Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 13 Nov 2024 14:06:50 -0800 Subject: [PATCH 4/5] docs --- tf2/include/tf2/buffer_core.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index 59070360e..80a35c607 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -229,7 +229,7 @@ class BufferCore : public BufferCoreInterface * \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 + * \param velocity The velocity output as a ROS type * * Possible exceptions TransformReference::LookupException, TransformReference::ConnectivityException, * TransformReference::MaxDepthException From 78b086f088ce6f3e7e0240c5beaa57ae7186b821 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 13 Nov 2024 14:19:38 -0800 Subject: [PATCH 5/5] remove pointless virtual --- tf2/include/tf2/buffer_core.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index 80a35c607..658b15b07 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -135,7 +135,7 @@ class BufferCore : public BufferCoreInterface * tf2::ExtrapolationException, tf2::InvalidArgumentException */ TF2_PUBLIC - virtual tf2::Stamped + tf2::Stamped lookupTransformTf2( const std::string & target_frame, const std::string & source_frame, @@ -170,7 +170,7 @@ class BufferCore : public BufferCoreInterface * tf2::ExtrapolationException, tf2::InvalidArgumentException */ TF2_PUBLIC - virtual tf2::Stamped + tf2::Stamped lookupTransformTf2( const std::string & target_frame, const tf2::TimePoint & target_time,