diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index 751e45c12..e9aafa097 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1371,9 +1371,50 @@ TEST(BufferCore_lookupTransform, multi_configuration) // } // +// Helpers used by gtest to print quaternions when errors happen +// Must be defined in the namespace the class to be printed in lives, as per +// https://github.com/google/googletest/blob/main/docs/advanced.md#teaching-googletest-how-to-print-your-values +namespace geometry_msgs::msg +{ +std::ostream & operator<<(std::ostream & stream, const Quaternion & q) +{ + stream << "{" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << "}"; + return stream; +} +} + +namespace tf2 +{ +std::ostream & operator<<(std::ostream & stream, const Quaternion & q) +{ + stream << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; + return stream; +} +} + +// Runs an equality check between a and b, allowing for the valid case that a == -b +// https://gamedev.stackexchange.com/questions/75072/how-can-i-compare-two-quaternions-for-logical-equality/75077#75077 +bool CheckQuaternionNear( + const geometry_msgs::msg::Quaternion & a, const tf2::Quaternion & b, + double epsilon) +{ + return ( + (std::abs(a.x - b.x()) < epsilon) && + (std::abs(a.y - b.y()) < epsilon) && + (std::abs(a.z - b.z()) < epsilon) && + (std::abs(a.w - b.w()) < epsilon) + ) || + ( + (std::abs(a.x + b.x()) < epsilon) && + (std::abs(a.y + b.y()) < epsilon) && + (std::abs(a.z + b.z()) < epsilon) && + (std::abs(a.w + b.w()) < epsilon) + ); +} + TEST(BufferCore_lookupTransform, ring_45_configuration) { - double epsilon = 1e-6; + const double epsilon = 1e-6; rostest::Permuter permuter; std::vector times; @@ -1456,10 +1497,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.x, sqrt(2) / 2 - 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, sqrt(2) / 2, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI / 8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI / 8), epsilon); + EXPECT_PRED3( + CheckQuaternionNear, outpose.transform.rotation, + tf2::Quaternion(0, 0, sin(M_PI / 8), cos(M_PI / 8)), epsilon); } // Inverse Chaining 1 else if ((source_frame == "b" && target_frame == "a") || @@ -1474,10 +1514,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.x, sqrt(2) / 2 - 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2) / 2, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI / 8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI / 8), epsilon); + EXPECT_PRED3( + CheckQuaternionNear, outpose.transform.rotation, + tf2::Quaternion(0, 0, sin(-M_PI / 8), cos(-M_PI / 8)), epsilon); } // Chaining 2 else if ((source_frame == "a" && target_frame == "c") || @@ -1491,10 +1530,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI / 4), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI / 4), epsilon); + EXPECT_PRED3( + CheckQuaternionNear, outpose.transform.rotation, + tf2::Quaternion(0, 0, sin(M_PI / 4), cos(M_PI / 4)), epsilon); } // Inverse Chaining 2 else if ((source_frame == "c" && target_frame == "a") || @@ -1508,10 +1546,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI / 4), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI / 4), epsilon); + EXPECT_PRED3( + CheckQuaternionNear, outpose.transform.rotation, + tf2::Quaternion(0, 0, sin(-M_PI / 4), cos(-M_PI / 4)), epsilon); } // Chaining 3 else if ((source_frame == "a" && target_frame == "d") || @@ -1524,10 +1561,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2) / 2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, sqrt(2) / 2, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI * 3 / 8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI * 3 / 8), epsilon); + EXPECT_PRED3( + CheckQuaternionNear, outpose.transform.rotation, + tf2::Quaternion(0, 0, sin(M_PI * 3 / 8), cos(M_PI * 3 / 8)), epsilon); } // Inverse Chaining 3 else if ((target_frame == "a" && source_frame == "d") || @@ -1540,10 +1576,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2) / 2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2) / 2, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, -sin(-M_PI * 3 / 8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, -cos(-M_PI * 3 / 8), epsilon); + EXPECT_PRED3( + CheckQuaternionNear, outpose.transform.rotation, + tf2::Quaternion(0, 0, -sin(-M_PI * 3 / 8), -cos(-M_PI * 3 / 8)), epsilon); } // Chaining 4 else if ((source_frame == "a" && target_frame == "e") || @@ -1555,10 +1590,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI / 2), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI / 2), epsilon); + EXPECT_PRED3( + CheckQuaternionNear, outpose.transform.rotation, + tf2::Quaternion(0, 0, sin(M_PI / 2), cos(M_PI / 2)), epsilon); } // Inverse Chaining 4 else if ((target_frame == "a" && source_frame == "e") || @@ -1570,10 +1604,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, -sin(-M_PI / 2), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI / 2), epsilon); + EXPECT_PRED3( + CheckQuaternionNear, outpose.transform.rotation, + tf2::Quaternion(0, 0, -sin(-M_PI / 2), cos(-M_PI / 2)), epsilon); } // Chaining 5 else if ((source_frame == "a" && target_frame == "f") || @@ -1584,10 +1617,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2) / 2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2) / 2, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI * 5 / 8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI * 5 / 8), epsilon); + EXPECT_PRED3( + CheckQuaternionNear, outpose.transform.rotation, + tf2::Quaternion(0, 0, sin(M_PI * 5 / 8), cos(M_PI * 5 / 8)), epsilon); } // Inverse Chaining 5 else if ((target_frame == "a" && source_frame == "f") || @@ -1598,10 +1630,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2) / 2, epsilon); EXPECT_NEAR(outpose.transform.translation.y, sqrt(2) / 2, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, -sin(-M_PI * 5 / 8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, -cos(-M_PI * 5 / 8), epsilon); + EXPECT_PRED3( + CheckQuaternionNear, outpose.transform.rotation, + tf2::Quaternion(0, 0, -sin(-M_PI * 5 / 8), -cos(-M_PI * 5 / 8)), epsilon); } // Chaining 6 else if ((source_frame == "a" && target_frame == "g") || @@ -1611,10 +1642,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, -sin(M_PI * 6 / 8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, -cos(M_PI * 6 / 8), epsilon); + EXPECT_PRED3( + CheckQuaternionNear, outpose.transform.rotation, + tf2::Quaternion(0, 0, -sin(M_PI * 6 / 8), -cos(M_PI * 6 / 8)), epsilon); } // Inverse Chaining 6 else if ((target_frame == "a" && source_frame == "g") || @@ -1624,10 +1654,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, -sin(-M_PI * 6 / 8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, -cos(-M_PI * 6 / 8), epsilon); + EXPECT_PRED3( + CheckQuaternionNear, outpose.transform.rotation, + tf2::Quaternion(0, 0, -sin(-M_PI * 6 / 8), -cos(-M_PI * 6 / 8)), epsilon); } // Chaining 7 else if ((source_frame == "a" && target_frame == "h") || @@ -1636,10 +1665,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.x, sqrt(2) / 2 - 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2) / 2, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI * 7 / 8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, -cos(-M_PI * 7 / 8), epsilon); + EXPECT_PRED3( + CheckQuaternionNear, outpose.transform.rotation, + tf2::Quaternion(0, 0, sin(-M_PI * 7 / 8), -cos(-M_PI * 7 / 8)), epsilon); } // Inverse Chaining 7 else if ((target_frame == "a" && source_frame == "h") || @@ -1648,10 +1676,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.x, sqrt(2) / 2 - 1, epsilon); EXPECT_NEAR(outpose.transform.translation.y, sqrt(2) / 2, epsilon); EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon); - EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI * 7 / 8), epsilon); - EXPECT_NEAR(outpose.transform.rotation.w, -cos(M_PI * 7 / 8), epsilon); + EXPECT_PRED3( + CheckQuaternionNear, outpose.transform.rotation, + tf2::Quaternion(0, 0, sin(M_PI * 7 / 8), -cos(M_PI * 7 / 8)), epsilon); } else { EXPECT_FALSE("Ring_45 testing Shouldn't get here"); printf( diff --git a/tf2/include/tf2/buffer_core.hpp b/tf2/include/tf2/buffer_core.hpp index 04c6f644e..0e363e5f6 100644 --- a/tf2/include/tf2/buffer_core.hpp +++ b/tf2/include/tf2/buffer_core.hpp @@ -44,13 +44,13 @@ #include #include -#include "LinearMath/Transform.hpp" +#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.hpp" -#include "tf2/exceptions.hpp" -#include "tf2/transform_storage.hpp" +#include "tf2/buffer_core_interface.h" +#include "tf2/exceptions.h" +#include "tf2/transform_storage.h" #include "tf2/visibility_control.h" namespace tf2 @@ -382,12 +382,16 @@ class BufferCore : public BufferCoreInterface std::string allFramesAsStringNoLock() const; bool setTransformImpl( - const tf2::Transform & transform_in, const std::string frame_id, - const std::string child_frame_id, const TimePoint stamp, + const tf2::Vector3 & origin_in, const tf2::Quaternion & rotation_in, + const std::string & frame_id, const std::string & child_frame_id, const TimePoint stamp, const std::string & authority, bool is_static); void lookupTransformImpl( const std::string & target_frame, const std::string & source_frame, - const TimePoint & time_in, tf2::Transform & transform, TimePoint & time_out) const; + const TimePoint & time_in, tf2::Transform & transform_out, TimePoint & time_out) const; + void lookupTransformImpl( + const std::string & target_frame, const std::string & source_frame, + const TimePoint & time_in, tf2::Vector3 & origin_out, tf2::Quaternion & rotation_out, + TimePoint & time_out) const; void lookupTransformImpl( const std::string & target_frame, const TimePoint & target_time, diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 887e637c4..863da44e5 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -182,27 +182,27 @@ bool BufferCore::setTransform( const geometry_msgs::msg::TransformStamped & transform, const std::string & authority, bool is_static) { - tf2::Transform tf2_transform(tf2::Quaternion( - transform.transform.rotation.x, - transform.transform.rotation.y, - transform.transform.rotation.z, - transform.transform.rotation.w), - tf2::Vector3( - transform.transform.translation.x, - transform.transform.translation.y, - transform.transform.translation.z)); + const tf2::Quaternion rotation( + transform.transform.rotation.x, + transform.transform.rotation.y, + transform.transform.rotation.z, + transform.transform.rotation.w); + const tf2::Vector3 origin( + transform.transform.translation.x, + transform.transform.translation.y, + transform.transform.translation.z); TimePoint time_point(std::chrono::nanoseconds(transform.header.stamp.nanosec) + std::chrono::duration_cast( std::chrono::seconds( transform.header.stamp.sec))); return setTransformImpl( - tf2_transform, transform.header.frame_id, transform.child_frame_id, + origin, rotation, transform.header.frame_id, transform.child_frame_id, time_point, authority, is_static); } bool BufferCore::setTransformImpl( - const tf2::Transform & transform_in, const std::string frame_id, - const std::string child_frame_id, const TimePoint stamp, + const tf2::Vector3 & origin_in, const tf2::Quaternion & rotation_in, const std::string & frame_id, + const std::string & child_frame_id, const TimePoint stamp, const std::string & authority, bool is_static) { std::string stripped_frame_id = stripSlash(frame_id); @@ -231,27 +231,27 @@ bool BufferCore::setTransformImpl( error_exists = true; } - if (std::isnan(transform_in.getOrigin().x()) || std::isnan(transform_in.getOrigin().y()) || - std::isnan(transform_in.getOrigin().z()) || - std::isnan(transform_in.getRotation().x()) || std::isnan(transform_in.getRotation().y()) || - std::isnan(transform_in.getRotation().z()) || std::isnan(transform_in.getRotation().w())) + if (std::isnan(origin_in.x()) || std::isnan(origin_in.y()) || + std::isnan(origin_in.z()) || + std::isnan(rotation_in.x()) || std::isnan(rotation_in.y()) || + std::isnan(rotation_in.z()) || std::isnan(rotation_in.w())) { RCUTILS_LOG_ERROR( "TF_NAN_INPUT: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because" " of a nan value in the transform (%f %f %f) (%f %f %f %f)", stripped_child_frame_id.c_str(), authority.c_str(), - transform_in.getOrigin().x(), transform_in.getOrigin().y(), transform_in.getOrigin().z(), - transform_in.getRotation().x(), transform_in.getRotation().y(), - transform_in.getRotation().z(), transform_in.getRotation().w() + origin_in.x(), origin_in.y(), origin_in.z(), + rotation_in.x(), rotation_in.y(), + rotation_in.z(), rotation_in.w() ); error_exists = true; } - bool valid = std::abs( - (transform_in.getRotation().w() * transform_in.getRotation().w() + - transform_in.getRotation().x() * transform_in.getRotation().x() + - transform_in.getRotation().y() * transform_in.getRotation().y() + - transform_in.getRotation().z() * transform_in.getRotation().z()) - 1.0f) < + const bool valid = std::abs( + (rotation_in.w() * rotation_in.w() + + rotation_in.x() * rotation_in.x() + + rotation_in.y() * rotation_in.y() + + rotation_in.z() * rotation_in.z()) - 1.0f) < QUATERNION_NORMALIZATION_TOLERANCE; if (!valid) { @@ -259,8 +259,8 @@ bool BufferCore::setTransformImpl( "TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id \"%s\" from authority" " \"%s\" because of an invalid quaternion in the transform (%f %f %f %f)", stripped_child_frame_id.c_str(), authority.c_str(), - transform_in.getRotation().x(), transform_in.getRotation().y(), - transform_in.getRotation().z(), transform_in.getRotation().w()); + rotation_in.x(), rotation_in.y(), + rotation_in.z(), rotation_in.w()); error_exists = true; } @@ -287,8 +287,8 @@ bool BufferCore::setTransformImpl( if (frame->insertData( TransformStorage( - stamp, transform_in.getRotation(), - transform_in.getOrigin(), lookupOrInsertFrameNumber(stripped_frame_id), frame_number))) + stamp, rotation_in, + origin_in, lookupOrInsertFrameNumber(stripped_frame_id), frame_number))) { frame_authority_[frame_number] = authority; } else { @@ -698,17 +698,18 @@ 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); + tf2::Vector3 origin; + tf2::Quaternion rotation; + lookupTransformImpl(target_frame, source_frame, time, origin, rotation, 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(); + msg.transform.translation.x = origin.x(); + msg.transform.translation.y = origin.y(); + msg.transform.translation.z = origin.z(); + msg.transform.rotation.x = rotation.x(); + msg.transform.rotation.y = rotation.y(); + msg.transform.rotation.z = rotation.z(); + msg.transform.rotation.w = rotation.w(); std::chrono::nanoseconds ns = std::chrono::duration_cast( time_out.time_since_epoch()); std::chrono::seconds s = std::chrono::duration_cast( @@ -736,10 +737,11 @@ BufferCore::lookupTransform( 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(); + tf2::Quaternion rotation = transform.getRotation(); + msg.transform.rotation.x = rotation.x(); + msg.transform.rotation.y = rotation.y(); + msg.transform.rotation.z = rotation.z(); + msg.transform.rotation.w = rotation.w(); std::chrono::nanoseconds ns = std::chrono::duration_cast( time_out.time_since_epoch()); std::chrono::seconds s = std::chrono::duration_cast( @@ -752,16 +754,56 @@ BufferCore::lookupTransform( return msg; } +/** \brief Private member function that looks up a transform between two frames + * at a given time and return the the transform as a TF2::transform. If a transform is + * not possible raise the appropriate error. + * \param target_frame -- the name of the target frame which we are transforming to + * \param source_frame -- the name of frame we are transforming from + * \param time -- the timepoint at which the transform should occur + * \param transform_out -- The transform, returned by reference, for the transform from source frame + * to target frame at the given time + * \param time_out -- the time the transform was computed returned by reference. + * \return void, the Transform between the input frames, and the time at which it was calculated are returned by reference + */ +void BufferCore::lookupTransformImpl( + const std::string & target_frame, + const std::string & source_frame, + const TimePoint & time, tf2::Transform & transform_out, + TimePoint & time_out) const +{ + tf2::Quaternion rotation; + lookupTransformImpl( + target_frame, source_frame, time, + transform_out.getOrigin(), rotation, time_out); + transform_out.setRotation(rotation); +} + +/** \brief Private member function that looks up a transform between two frames + * at a given time and return the the transform as a TF2::transform. If a transform is + * not possible raise the appropriate error. The rotation and orientation + * components are returned separately by reference. + * \param target_frame -- the name of the target frame which we are transforming to + * \param source_frame -- the name of frame we are transforming from + * \param time -- the timepoint at which the transform should occur + * \param origin_out -- the position component of the desired transform + * passed by reference + * \param origin_out -- the position component of the desired transform + * passed by reference + * \param rotation_out -- the rotation component of the desired transform + * passed by reference + * \return void, the Transform between the input frames, and the time at which it was calculated are returned by reference + */ void BufferCore::lookupTransformImpl( const std::string & target_frame, const std::string & source_frame, - const TimePoint & time, tf2::Transform & transform, + const TimePoint & time, tf2::Vector3 & origin_out, tf2::Quaternion & rotation_out, TimePoint & time_out) const { std::unique_lock lock(frame_mutex_); if (target_frame == source_frame) { - transform.setIdentity(); + rotation_out = Quaternion::getIdentity(); + origin_out.setValue(tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0)); if (time == TimePointZero) { CompactFrameID target_id = lookupFrameNumber(target_frame); @@ -805,8 +847,8 @@ void BufferCore::lookupTransformImpl( } time_out = accum.time; - transform.setOrigin(accum.result_vec); - transform.setRotation(accum.result_quat); + origin_out = accum.result_vec; + rotation_out = accum.result_quat; } void BufferCore::lookupTransformImpl( diff --git a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp index 784fa31fe..042e5a82f 100644 --- a/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp +++ b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp @@ -853,10 +853,11 @@ geometry_msgs::msg::PoseWithCovarianceStamped toMsg( out.header.stamp = tf2_ros::toMsg(in.stamp_); out.header.frame_id = in.frame_id_; out.pose.covariance = covarianceNestedToRowMajor(in.cov_mat_); - out.pose.pose.orientation.x = in.getRotation().getX(); - out.pose.pose.orientation.y = in.getRotation().getY(); - out.pose.pose.orientation.z = in.getRotation().getZ(); - out.pose.pose.orientation.w = in.getRotation().getW(); + const tf2::Quaternion rotation = in.getRotation(); + out.pose.pose.orientation.x = rotation.getX(); + out.pose.pose.orientation.y = rotation.getY(); + out.pose.pose.orientation.z = rotation.getZ(); + out.pose.pose.orientation.w = rotation.getW(); out.pose.pose.position.x = in.getOrigin().getX(); out.pose.pose.position.y = in.getOrigin().getY(); out.pose.pose.position.z = in.getOrigin().getZ(); @@ -1339,15 +1340,17 @@ void doTransform( 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)); + 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; diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 069965a0c..cdcdafdd5 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -67,6 +67,47 @@ geometry_msgs::msg::TransformStamped generate_stamped_transform() return t; } +// Helpers used by gtest to print quaternions when errors happen +// Must be defined in the namespace the class to be printed in lives, as per +// https://github.com/google/googletest/blob/main/docs/advanced.md#teaching-googletest-how-to-print-your-values +namespace geometry_msgs::msg +{ +std::ostream & operator<<(std::ostream & stream, const Quaternion & q) +{ + stream << "{" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << "}"; + return stream; +} +} + +namespace tf2 +{ +std::ostream & operator<<(std::ostream & stream, const Quaternion & q) +{ + stream << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; + return stream; +} +} + +// Runs an equality check between a and b, allowing for the valid case that a == -b +// https://gamedev.stackexchange.com/questions/75072/how-can-i-compare-two-quaternions-for-logical-equality/75077#75077 +bool CheckQuaternionNear( + const geometry_msgs::msg::Quaternion & a, + const tf2::Quaternion & b, double epsilon) +{ + return ( + (std::abs(a.x - b.x()) < epsilon) && + (std::abs(a.y - b.y()) < epsilon) && + (std::abs(a.z - b.z()) < epsilon) && + (std::abs(a.w - b.w()) < epsilon) + ) || + ( + (std::abs(a.x + b.x()) < epsilon) && + (std::abs(a.y + b.y()) < epsilon) && + (std::abs(a.z + b.z()) < epsilon) && + (std::abs(a.w + b.w()) < epsilon) + ); +} + TEST(TfGeometry, Conversions) { // Quaternion @@ -123,10 +164,9 @@ TEST(TfGeometry, Conversions) geometry_msgs::msg::Transform tf_msg; tf2::convert(tf_, tf_msg); - EXPECT_NEAR(rotation.getX(), tf_msg.rotation.x, EPS); - EXPECT_NEAR(rotation.getY(), tf_msg.rotation.y, EPS); - EXPECT_NEAR(rotation.getZ(), tf_msg.rotation.z, EPS); - EXPECT_NEAR(rotation.getW(), tf_msg.rotation.w, EPS); + // Calls function in first position, with arguments in second and third + // position along with epsilon value defined in the fourth position. + EXPECT_PRED3(CheckQuaternionNear, tf_msg.rotation, rotation, EPS); EXPECT_NEAR(translation.getX(), tf_msg.translation.x, EPS); EXPECT_NEAR(translation.getY(), tf_msg.translation.y, EPS); EXPECT_NEAR(translation.getZ(), tf_msg.translation.z, EPS); @@ -134,10 +174,7 @@ TEST(TfGeometry, Conversions) tf2::Transform tf_from_msg; tf2::convert(tf_msg, tf_from_msg); - EXPECT_NEAR(tf_from_msg.getRotation().getX(), tf_msg.rotation.x, EPS); - EXPECT_NEAR(tf_from_msg.getRotation().getY(), tf_msg.rotation.y, EPS); - EXPECT_NEAR(tf_from_msg.getRotation().getZ(), tf_msg.rotation.z, EPS); - EXPECT_NEAR(tf_from_msg.getRotation().getW(), tf_msg.rotation.w, EPS); + EXPECT_PRED3(CheckQuaternionNear, tf_msg.rotation, tf_from_msg.getRotation(), EPS); EXPECT_NEAR(tf_from_msg.getOrigin().getX(), tf_msg.translation.x, EPS); EXPECT_NEAR(tf_from_msg.getOrigin().getY(), tf_msg.translation.y, EPS); EXPECT_NEAR(tf_from_msg.getOrigin().getZ(), tf_msg.translation.z, EPS); @@ -153,10 +190,7 @@ TEST(TfGeometry, Conversions) geometry_msgs::msg::TransformStamped tf_stamped_msg; tf2::convert(tf_stamped, tf_stamped_msg); - EXPECT_NEAR(rotation.getX(), tf_stamped_msg.transform.rotation.x, EPS); - EXPECT_NEAR(rotation.getY(), tf_stamped_msg.transform.rotation.y, EPS); - EXPECT_NEAR(rotation.getZ(), tf_stamped_msg.transform.rotation.z, EPS); - EXPECT_NEAR(rotation.getW(), tf_stamped_msg.transform.rotation.w, EPS); + EXPECT_PRED3(CheckQuaternionNear, tf_stamped_msg.transform.rotation, rotation, EPS); EXPECT_NEAR(translation.getX(), tf_stamped_msg.transform.translation.x, EPS); EXPECT_NEAR(translation.getY(), tf_stamped_msg.transform.translation.y, EPS); EXPECT_NEAR(translation.getZ(), tf_stamped_msg.transform.translation.z, EPS); @@ -165,10 +199,7 @@ TEST(TfGeometry, Conversions) tf2::Stamped tf_from_msg; tf2::convert(tf_stamped_msg, tf_from_msg); - EXPECT_NEAR(tf_from_msg.getRotation().getX(), tf_stamped_msg.transform.rotation.x, EPS); - EXPECT_NEAR(tf_from_msg.getRotation().getY(), tf_stamped_msg.transform.rotation.y, EPS); - EXPECT_NEAR(tf_from_msg.getRotation().getZ(), tf_stamped_msg.transform.rotation.z, EPS); - EXPECT_NEAR(tf_from_msg.getRotation().getW(), tf_stamped_msg.transform.rotation.w, EPS); + EXPECT_PRED3(CheckQuaternionNear, tf_stamped_msg.transform.rotation, rotation, EPS); EXPECT_NEAR(tf_from_msg.getOrigin().getX(), tf_stamped_msg.transform.translation.x, EPS); EXPECT_NEAR(tf_from_msg.getOrigin().getY(), tf_stamped_msg.transform.translation.y, EPS); EXPECT_NEAR(tf_from_msg.getOrigin().getZ(), tf_stamped_msg.transform.translation.z, EPS); @@ -520,22 +551,20 @@ TEST(TfGeometry, Quaternion) q1.header.frame_id = "A"; // simple api - const geometry_msgs::msg::QuaternionStamped q_simple = tf_buffer->transform( - q1, "B", tf2::durationFromSec( - 2.0)); - EXPECT_NEAR(q_simple.quaternion.x, M_SQRT1_2, EPS); - EXPECT_NEAR(q_simple.quaternion.y, 0, EPS); - EXPECT_NEAR(q_simple.quaternion.z, -1 * M_SQRT1_2, EPS); - EXPECT_NEAR(q_simple.quaternion.w, 0, EPS); + { + const geometry_msgs::msg::QuaternionStamped q_simple = tf_buffer->transform( + q1, "B", tf2::durationFromSec(2.0)); + const tf2::Quaternion q_simple_should_equal(M_SQRT1_2, 0, -M_SQRT1_2, 0); + EXPECT_PRED3(CheckQuaternionNear, q_simple.quaternion, q_simple_should_equal, EPS); + } // advanced api - const geometry_msgs::msg::QuaternionStamped q_advanced = tf_buffer->transform( - q1, "B", tf2::timeFromSec(2.0), - "A", tf2::durationFromSec(3.0)); - EXPECT_NEAR(q_advanced.quaternion.x, M_SQRT1_2, EPS); - EXPECT_NEAR(q_advanced.quaternion.y, 0, EPS); - EXPECT_NEAR(q_advanced.quaternion.z, -1 * M_SQRT1_2, EPS); - EXPECT_NEAR(q_advanced.quaternion.w, 0, EPS); + { + const geometry_msgs::msg::QuaternionStamped q_advanced = tf_buffer->transform( + q1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0)); + tf2::Quaternion q_advanced_should_equal(M_SQRT1_2, 0, -M_SQRT1_2, 0); + EXPECT_PRED3(CheckQuaternionNear, q_advanced.quaternion, q_advanced_should_equal, EPS); + } } }