From 246fa848c42f9b9140c2ab414e98a875e81f1325 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Tue, 3 Dec 2024 15:16:32 -0800 Subject: [PATCH 01/44] fix repeated calls to Transform::getRotation --- tf2/src/buffer_core.cpp | 24 ++++++++++--------- .../tf2_geometry_msgs/tf2_geometry_msgs.hpp | 9 +++---- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index f4333729c..5d6af8868 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -231,27 +231,29 @@ bool BufferCore::setTransformImpl( error_exists = true; } + const tf2::Quaternion rotation_in = transform_in.getRotation(); + 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())) + 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() + 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 +261,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; } 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 2a60fa3ae..3ac43f6bd 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(); + 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(); From e3ccee1bf6eb2c037ae106e0dda62389af7c5d00 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Tue, 3 Dec 2024 15:25:20 -0800 Subject: [PATCH 02/44] pick up one more place --- tf2/src/buffer_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 5d6af8868..3f6cff36c 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -289,7 +289,7 @@ bool BufferCore::setTransformImpl( if (frame->insertData( TransformStorage( - stamp, transform_in.getRotation(), + stamp, rotation_in, transform_in.getOrigin(), lookupOrInsertFrameNumber(stripped_frame_id), frame_number))) { frame_authority_[frame_number] = authority; From e79133146863bbdb4601c77d59d66f312f4882c8 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Tue, 3 Dec 2024 15:37:47 -0800 Subject: [PATCH 03/44] add new setTransformImpl --- tf2/include/tf2/buffer_core.h | 5 +++++ tf2/src/buffer_core.cpp | 16 ++++++++++++---- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index 0c949e944..7862a107d 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -381,10 +381,15 @@ class BufferCore : public BufferCoreInterface */ std::string allFramesAsStringNoLock() const; + [[deprecated("Pass origin and rotation separately")]] bool setTransformImpl( const tf2::Transform & transform_in, const std::string frame_id, const std::string child_frame_id, const TimePoint stamp, const std::string & authority, bool is_static); + bool setTransformImpl( + 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; diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 3f6cff36c..863739cde 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -182,21 +182,21 @@ bool BufferCore::setTransform( const geometry_msgs::msg::TransformStamped & transform, const std::string & authority, bool is_static) { - tf2::Transform tf2_transform(tf2::Quaternion( + tf2::Quaternion rotation( transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w), - tf2::Vector3( + tf2::Vector3 origin( transform.transform.translation.x, transform.transform.translation.y, - transform.transform.translation.z)); + 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, + rotation, origin, transform.header.frame_id, transform.child_frame_id, time_point, authority, is_static); } @@ -204,6 +204,14 @@ bool BufferCore::setTransformImpl( const tf2::Transform & transform_in, const std::string frame_id, const std::string child_frame_id, const TimePoint stamp, const std::string & authority, bool is_static) +{ + return setTransformImpl(transform_in.getOrigin(), transform_in.getRotation(), frame_id, child_frame_id, stamp, authority, is_static); +} + +bool BufferCore::setTransformImpl( + 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); std::string stripped_child_frame_id = stripSlash(child_frame_id); From 51f75b46ed4d129d2f38d70b5ad4dbf2d9a9f5c4 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Tue, 3 Dec 2024 15:45:43 -0800 Subject: [PATCH 04/44] change setTransformImpl to take origin and rotation --- tf2/include/tf2/buffer_core.h | 10 +++---- tf2/src/buffer_core.cpp | 28 +++++++++---------- .../tf2_geometry_msgs/tf2_geometry_msgs.hpp | 2 +- 3 files changed, 19 insertions(+), 21 deletions(-) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index 7862a107d..53ff567e3 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -381,11 +381,11 @@ class BufferCore : public BufferCoreInterface */ std::string allFramesAsStringNoLock() const; - [[deprecated("Pass origin and rotation separately")]] - bool setTransformImpl( - const tf2::Transform & transform_in, const std::string frame_id, - const std::string child_frame_id, const TimePoint stamp, - const std::string & authority, bool is_static); + // [[deprecated("Pass origin and rotation separately")]] + // bool setTransformImpl( + // const tf2::Transform & transform_in, const std::string frame_id, + // const std::string child_frame_id, const TimePoint stamp, + // const std::string & authority, bool is_static); bool setTransformImpl( const tf2::Vector3 & origin_in, const tf2::Quaternion & rotation_in, const std::string & frame_id, const std::string child_frame_id, const TimePoint stamp, diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 863739cde..00548704c 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -186,7 +186,7 @@ bool BufferCore::setTransform( transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, - transform.transform.rotation.w), + transform.transform.rotation.w); tf2::Vector3 origin( transform.transform.translation.x, transform.transform.translation.y, @@ -196,17 +196,17 @@ bool BufferCore::setTransform( std::chrono::seconds( transform.header.stamp.sec))); return setTransformImpl( - rotation, origin, 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 std::string & authority, bool is_static) -{ - return setTransformImpl(transform_in.getOrigin(), transform_in.getRotation(), frame_id, child_frame_id, stamp, 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 std::string & authority, bool is_static) +// { +// return setTransformImpl(transform_in.getOrigin(), transform_in.getRotation(), frame_id, child_frame_id, stamp, authority, is_static); +// } bool BufferCore::setTransformImpl( const tf2::Vector3 & origin_in, const tf2::Quaternion & rotation_in, const std::string & frame_id, @@ -239,10 +239,8 @@ bool BufferCore::setTransformImpl( error_exists = true; } - const tf2::Quaternion rotation_in = transform_in.getRotation(); - - if (std::isnan(transform_in.getOrigin().x()) || std::isnan(transform_in.getOrigin().y()) || - std::isnan(transform_in.getOrigin().z()) || + 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())) { @@ -250,7 +248,7 @@ bool BufferCore::setTransformImpl( "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(), + origin_in.x(), origin_in.y(), origin_in.z(), rotation_in.x(), rotation_in.y(), rotation_in.z(), rotation_in.w() ); @@ -298,7 +296,7 @@ bool BufferCore::setTransformImpl( if (frame->insertData( TransformStorage( stamp, rotation_in, - transform_in.getOrigin(), lookupOrInsertFrameNumber(stripped_frame_id), frame_number))) + origin_in, lookupOrInsertFrameNumber(stripped_frame_id), frame_number))) { frame_authority_[frame_number] = authority; } else { 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 3ac43f6bd..1e387653b 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,7 +853,7 @@ 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_); - tf2::Quaternion rotation - in.getRotation(); + 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(); From 32660cab18b776603c73769f333f135791fd4c22 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Tue, 3 Dec 2024 15:46:59 -0800 Subject: [PATCH 05/44] remove deleted code --- tf2/include/tf2/buffer_core.h | 5 ----- tf2/src/buffer_core.cpp | 8 -------- 2 files changed, 13 deletions(-) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index 53ff567e3..c70a24d22 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -381,11 +381,6 @@ class BufferCore : public BufferCoreInterface */ std::string allFramesAsStringNoLock() const; - // [[deprecated("Pass origin and rotation separately")]] - // bool setTransformImpl( - // const tf2::Transform & transform_in, const std::string frame_id, - // const std::string child_frame_id, const TimePoint stamp, - // const std::string & authority, bool is_static); bool setTransformImpl( const tf2::Vector3 & origin_in, const tf2::Quaternion & rotation_in, const std::string & frame_id, const std::string child_frame_id, const TimePoint stamp, diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 00548704c..8debebb79 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -200,14 +200,6 @@ bool BufferCore::setTransform( 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 std::string & authority, bool is_static) -// { -// return setTransformImpl(transform_in.getOrigin(), transform_in.getRotation(), frame_id, child_frame_id, stamp, authority, is_static); -// } - bool BufferCore::setTransformImpl( const tf2::Vector3 & origin_in, const tf2::Quaternion & rotation_in, const std::string & frame_id, const std::string child_frame_id, const TimePoint stamp, From 46d66fc6beefcba5f17abcbe62b803f4c40e7d59 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Tue, 3 Dec 2024 15:47:53 -0800 Subject: [PATCH 06/44] fix missing reference --- tf2/include/tf2/buffer_core.h | 2 +- tf2/src/buffer_core.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index c70a24d22..a9d7828a9 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -383,7 +383,7 @@ class BufferCore : public BufferCoreInterface bool setTransformImpl( 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 & 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, diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 8debebb79..ee2dd3617 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -202,7 +202,7 @@ bool BufferCore::setTransform( bool BufferCore::setTransformImpl( 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 & child_frame_id, const TimePoint stamp, const std::string & authority, bool is_static) { std::string stripped_frame_id = stripSlash(frame_id); From 03616240cbdc38c389252821bac3592383437d77 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Tue, 3 Dec 2024 16:13:32 -0800 Subject: [PATCH 07/44] add vector+quat interface to lookupTransformImpl --- tf2/include/tf2/buffer_core.h | 5 +++- tf2/src/buffer_core.cpp | 47 +++++++++++++++++++++++------------ 2 files changed, 35 insertions(+), 17 deletions(-) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index a9d7828a9..214c333bf 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -387,7 +387,10 @@ class BufferCore : public BufferCoreInterface 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 ee2dd3617..2ebf9cb4c 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -700,15 +700,17 @@ BufferCore::lookupTransform( { 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 +738,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( @@ -755,13 +758,25 @@ BufferCore::lookupTransform( void BufferCore::lookupTransformImpl( const std::string & target_frame, const std::string & source_frame, - const TimePoint & time, tf2::Transform & transform, + 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); +} + +void BufferCore::lookupTransformImpl( + const std::string & target_frame, + const std::string & source_frame, + 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 +820,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( From a97924505d62f3e0e90985fca54c9bf4b51d2a60 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Tue, 3 Dec 2024 16:32:10 -0800 Subject: [PATCH 08/44] fix tabs --- tf2/src/buffer_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 2ebf9cb4c..db5c8b98f 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -776,7 +776,7 @@ void BufferCore::lookupTransformImpl( if (target_frame == source_frame) { rotation_out = Quaternion::getIdentity(); - origin_out.setValue(tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0)); + origin_out.setValue(tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0)); if (time == TimePointZero) { CompactFrameID target_id = lookupFrameNumber(target_frame); From 7e0f4b34ba4bfe67fca42e717d2e7dcadfd6ece0 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 00:11:07 -0800 Subject: [PATCH 09/44] fix tests, I hope --- test_tf2/test/buffer_core_test.cpp | 68 +++++++++++++----------------- 1 file changed, 30 insertions(+), 38 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index ad3ec28c9..1b5e91448 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1371,6 +1371,22 @@ TEST(BufferCore_lookupTransform, multi_configuration) // } // +bool CheckQuaternionNear(const geometry_msgs::msg::Quaternion & a, const tf2::Quaternion & b, double epsilon) +{ + return ( + (a.x - b.x() < epsilon) && + (a.y - b.y() < epsilon) && + (a.z - b.z() < epsilon) && + (a.w - b.w() < epsilon) + ) || + ( + (a.x + b.x() < epsilon) && + (a.y + b.y() < epsilon) && + (a.z + b.z() < epsilon) && + (a.w + b.w() < epsilon) + ); +} + TEST(BufferCore_lookupTransform, ring_45_configuration) { double epsilon = 1e-6; @@ -1458,8 +1474,7 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) 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 +1489,7 @@ 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 +1503,7 @@ 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 +1517,7 @@ 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 +1530,7 @@ 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 +1543,7 @@ 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") || @@ -1557,8 +1557,7 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) 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") || @@ -1572,8 +1571,7 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) 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") || @@ -1586,8 +1584,7 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) 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") || @@ -1600,8 +1597,7 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) 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") || @@ -1613,8 +1609,7 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) 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") || @@ -1626,8 +1621,7 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) 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") || @@ -1638,8 +1632,7 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) 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") || @@ -1650,8 +1643,7 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) 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 * 6 / 8), -cos(M_PI * 6 / 8)), epsilon);; } else { EXPECT_FALSE("Ring_45 testing Shouldn't get here"); printf( From dc2f979877bcd3974bf1da5b5c941da3325f5996 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 00:31:40 -0800 Subject: [PATCH 10/44] uncrustify --- tf2/include/tf2/buffer_core.h | 7 ++++--- .../tf2_geometry_msgs/tf2_geometry_msgs.hpp | 20 ++++++++++--------- 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h index 214c333bf..97c6e1bf3 100644 --- a/tf2/include/tf2/buffer_core.h +++ b/tf2/include/tf2/buffer_core.h @@ -382,15 +382,16 @@ class BufferCore : public BufferCoreInterface std::string allFramesAsStringNoLock() const; bool setTransformImpl( - 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 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_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; + 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_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp b/tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp index 1e387653b..0687d9032 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 @@ -1340,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; From 0030dfb015774fe354b74921620c09460c4ab674 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 02:07:17 -0800 Subject: [PATCH 11/44] fix extraneous lines --- test_tf2/test/buffer_core_test.cpp | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index 1b5e91448..2f67785e1 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1472,8 +1472,6 @@ 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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, sin(M_PI / 8), cos(M_PI / 8)), epsilon); } // Inverse Chaining 1 @@ -1555,8 +1553,6 @@ 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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, sin(M_PI / 2), cos(M_PI / 2)), epsilon); } // Inverse Chaining 4 @@ -1569,8 +1565,6 @@ 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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, -sin(-M_PI / 2), cos(-M_PI / 2)), epsilon); } // Chaining 5 @@ -1582,8 +1576,6 @@ 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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, sin(M_PI * 5 / 8), cos(M_PI * 5 / 8)), epsilon); } // Inverse Chaining 5 @@ -1595,8 +1587,6 @@ 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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, -sin(-M_PI * 5 / 8), -cos(-M_PI * 5 / 8)), epsilon); } // Chaining 6 @@ -1607,8 +1597,6 @@ 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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, -sin(M_PI * 6 / 8), -cos(M_PI * 6 / 8)), epsilon); } // Inverse Chaining 6 @@ -1619,8 +1607,6 @@ 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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, -sin(-M_PI * 6 / 8), -cos(-M_PI * 6 / 8)), epsilon); } // Chaining 7 @@ -1630,8 +1616,6 @@ 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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, sin(-M_PI * 7 / 8), -cos(-M_PI * 7 / 8)), epsilon); } // Inverse Chaining 7 @@ -1641,8 +1625,6 @@ 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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, sin(M_PI * 6 / 8), -cos(M_PI * 6 / 8)), epsilon);; } else { EXPECT_FALSE("Ring_45 testing Shouldn't get here"); From d524ed8c817b3d50235e79402a76ec43b9aa7eb0 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 02:09:39 -0800 Subject: [PATCH 12/44] add comment, std abs --- test_tf2/test/buffer_core_test.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index 2f67785e1..9a169c816 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1371,19 +1371,20 @@ TEST(BufferCore_lookupTransform, multi_configuration) // } // +// Runs an equality check between a and b, allowing for the valid case that a == -b bool CheckQuaternionNear(const geometry_msgs::msg::Quaternion & a, const tf2::Quaternion & b, double epsilon) { return ( - (a.x - b.x() < epsilon) && - (a.y - b.y() < epsilon) && - (a.z - b.z() < epsilon) && - (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) ) || ( - (a.x + b.x() < epsilon) && - (a.y + b.y() < epsilon) && - (a.z + b.z() < epsilon) && - (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) ); } From 8bd813a701a0844c4fbd0420fb73cefd7c1a1b3b Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 02:10:11 -0800 Subject: [PATCH 13/44] remove unused transform --- tf2/src/buffer_core.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index db5c8b98f..fb69d0954 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -698,7 +698,6 @@ BufferCore::lookupTransform( const std::string & target_frame, const std::string & source_frame, const TimePoint & time) const { - tf2::Transform transform; TimePoint time_out; tf2::Vector3 origin; tf2::Quaternion rotation; From 77c8010ad781f4bef8d95cf51a256ef519df1220 Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Wed, 4 Dec 2024 02:11:39 -0800 Subject: [PATCH 14/44] Update tf2/src/buffer_core.cpp Co-authored-by: jmachowinski Signed-off-by: kyle-basis --- tf2/src/buffer_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index fb69d0954..2d8b94768 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -737,7 +737,7 @@ BufferCore::lookupTransform( msg.transform.translation.x = transform.getOrigin().x(); msg.transform.translation.y = transform.getOrigin().y(); msg.transform.translation.z = transform.getOrigin().z(); - tf2::Quaternion rotation = transform.getRotation(); + const tf2::Quaternion rotation = transform.getRotation(); msg.transform.rotation.x = rotation.x(); msg.transform.rotation.y = rotation.y(); msg.transform.rotation.z = rotation.z(); From b399f8f4df0e3091c0b03885f2460e0ebf98464e Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 13:37:14 -0800 Subject: [PATCH 15/44] fix another test --- test_tf2/test/buffer_core_test.cpp | 1 + .../test/test_tf2_geometry_msgs.cpp | 38 +++++++++++-------- 2 files changed, 23 insertions(+), 16 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index 9a169c816..5697922c6 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1372,6 +1372,7 @@ TEST(BufferCore_lookupTransform, multi_configuration) // // 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 ( diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 069965a0c..2ec13e0c4 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -67,6 +67,24 @@ geometry_msgs::msg::TransformStamped generate_stamped_transform() return t; } +// 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 +141,7 @@ 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); + 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 +149,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 +165,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 +174,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); From bc5db14f6df8a86094d9cda574f07345d280f046 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 14:01:20 -0800 Subject: [PATCH 16/44] yet another failing test --- .../test/test_tf2_geometry_msgs.cpp | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 2ec13e0c4..c5b914ed8 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -526,22 +526,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)); + EXPECT_PRED3(CheckQuaternionNear, q_simple.quaternion, tf2::Quaternion(M_SQRT1_2, 0, -M_SQRT1_2, 0), 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)); + EXPECT_PRED3(CheckQuaternionNear, q_advanced.quaternion, tf2::Quaternion(M_SQRT1_2, 0, -M_SQRT1_2, 0), EPS); + } } } From be1ba27a93765a868e4a01f8d71cbc6733a8ace6 Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Wed, 4 Dec 2024 17:43:02 -0800 Subject: [PATCH 17/44] Update tf2/src/buffer_core.cpp Co-authored-by: Katherine Scott Signed-off-by: kyle-basis --- tf2/src/buffer_core.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 2d8b94768..88ecc3bdc 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -765,6 +765,21 @@ void BufferCore::lookupTransformImpl( 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, From 86c61810f30e237915a56be44ec44e934bf337e2 Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Wed, 4 Dec 2024 17:43:36 -0800 Subject: [PATCH 18/44] Update tf2/src/buffer_core.cpp Co-authored-by: Katherine Scott Signed-off-by: kyle-basis --- tf2/src/buffer_core.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 88ecc3bdc..d332540e1 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -754,6 +754,17 @@ 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, From bb368e5577fc13b9c0d7e4c00500b86e2f72e97b Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Wed, 4 Dec 2024 17:45:38 -0800 Subject: [PATCH 19/44] Update tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp Co-authored-by: Katherine Scott Signed-off-by: kyle-basis --- tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index c5b914ed8..d4ce103ab 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -141,6 +141,8 @@ TEST(TfGeometry, Conversions) geometry_msgs::msg::Transform tf_msg; tf2::convert(tf_, tf_msg); + // 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); From a2e08e6c644b6af48cfaa47a711c39ae6f2fb703 Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Wed, 4 Dec 2024 21:30:42 -0800 Subject: [PATCH 20/44] Update tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp Co-authored-by: Katherine Scott Signed-off-by: kyle-basis --- tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index d4ce103ab..1bcaccf69 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -540,7 +540,8 @@ TEST(TfGeometry, Quaternion) const geometry_msgs::msg::QuaternionStamped q_advanced = tf_buffer->transform( q1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0)); - EXPECT_PRED3(CheckQuaternionNear, q_advanced.quaternion, tf2::Quaternion(M_SQRT1_2, 0, -M_SQRT1_2, 0), EPS); + EXPECT_PRED3(CheckQuaternionNear, q_advanced.quaternion, + tf2::Quaternion(M_SQRT1_2, 0, -M_SQRT1_2, 0), EPS); } } } From 5d338253f7ccf6cf70e8bd0f4d75e2eca5e41a15 Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Wed, 4 Dec 2024 21:31:33 -0800 Subject: [PATCH 21/44] Apply suggestions from code review Co-authored-by: Katherine Scott Signed-off-by: kyle-basis --- tf2/src/buffer_core.cpp | 3 ++- tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp | 6 ++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index d332540e1..3e09e85e2 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -772,7 +772,8 @@ void BufferCore::lookupTransformImpl( TimePoint & time_out) const { tf2::Quaternion rotation; - lookupTransformImpl(target_frame, source_frame, time, transform_out.getOrigin(), rotation, time_out); + lookupTransformImpl(target_frame, source_frame, time, + transform_out.getOrigin(), rotation, time_out); transform_out.setRotation(rotation); } diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 1bcaccf69..2ded59795 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -69,7 +69,8 @@ geometry_msgs::msg::TransformStamped generate_stamped_transform() // 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) +bool CheckQuaternionNear(const geometry_msgs::msg::Quaternion & a, + const tf2::Quaternion & b, double epsilon) { return ( (std::abs(a.x - b.x()) < epsilon) && @@ -532,7 +533,8 @@ TEST(TfGeometry, Quaternion) const geometry_msgs::msg::QuaternionStamped q_simple = tf_buffer->transform( q1, "B", tf2::durationFromSec( 2.0)); - EXPECT_PRED3(CheckQuaternionNear, q_simple.quaternion, tf2::Quaternion(M_SQRT1_2, 0, -M_SQRT1_2, 0), EPS); + EXPECT_PRED3(CheckQuaternionNear, q_simple.quaternion, + tf2::Quaternion(M_SQRT1_2, 0, -M_SQRT1_2, 0), EPS); } // advanced api From edb14fecc0fda2fa3e1ced3eac06d5f1b4b35c20 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 21:31:51 -0800 Subject: [PATCH 22/44] one more const --- tf2/src/buffer_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 2d8b94768..fb69d0954 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -737,7 +737,7 @@ BufferCore::lookupTransform( msg.transform.translation.x = transform.getOrigin().x(); msg.transform.translation.y = transform.getOrigin().y(); msg.transform.translation.z = transform.getOrigin().z(); - const tf2::Quaternion rotation = transform.getRotation(); + tf2::Quaternion rotation = transform.getRotation(); msg.transform.rotation.x = rotation.x(); msg.transform.rotation.y = rotation.y(); msg.transform.rotation.z = rotation.z(); From ca99cf9ceaea891330558d477382dfc42d633134 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 21:34:38 -0800 Subject: [PATCH 23/44] readibility --- tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 2ded59795..ac309c71a 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -531,19 +531,17 @@ TEST(TfGeometry, Quaternion) // simple api { const geometry_msgs::msg::QuaternionStamped q_simple = tf_buffer->transform( - q1, "B", tf2::durationFromSec( - 2.0)); - EXPECT_PRED3(CheckQuaternionNear, q_simple.quaternion, - tf2::Quaternion(M_SQRT1_2, 0, -M_SQRT1_2, 0), EPS); + 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_PRED3(CheckQuaternionNear, q_advanced.quaternion, - tf2::Quaternion(M_SQRT1_2, 0, -M_SQRT1_2, 0), EPS); + 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); } } } From 095195cf08d30299a092ca9af4ba8d8e0144028f Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 21:48:02 -0800 Subject: [PATCH 24/44] uncrustify --- .../test/test_tf2_geometry_msgs.cpp | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index ac309c71a..452fc150d 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -69,21 +69,22 @@ geometry_msgs::msg::TransformStamped generate_stamped_transform() // 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) +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) - ); + (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) @@ -142,7 +143,7 @@ TEST(TfGeometry, Conversions) geometry_msgs::msg::Transform tf_msg; tf2::convert(tf_, tf_msg); - // Calls function in first position, with arguments in second and third + // 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); From eaa507b5b8dcb19f4c4d54e1cbcef898d04b747e Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 21:56:55 -0800 Subject: [PATCH 25/44] add helpers to print --- test_tf2/test/buffer_core_test.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index 5697922c6..e1ed99081 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1371,6 +1371,21 @@ TEST(BufferCore_lookupTransform, multi_configuration) // } // +// Helpers to print quaternions for tests +// 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 { + void PrintTo(const Quaternion& q, std::ostream* os) { + *os << "{" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << "}"; + } +} + +namespace tf2 { + void PrintTo(const Quaternion& q, std::ostream* os) { + *os << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; + } +} + // 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) From 6774cb60c282f065c0674681d57e006a390f9f32 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 21:58:56 -0800 Subject: [PATCH 26/44] uncrustify --- test_tf2/test/buffer_core_test.cpp | 102 +++++++++++++++++++---------- 1 file changed, 68 insertions(+), 34 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index e1ed99081..078695459 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1374,34 +1374,40 @@ TEST(BufferCore_lookupTransform, multi_configuration) // Helpers to print quaternions for tests // 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 { - void PrintTo(const Quaternion& q, std::ostream* os) { - *os << "{" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << "}"; - } +namespace geometry_msgs::msg +{ +void PrintTo(const Quaternion & q, std::ostream * os) +{ + *os << "{" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << "}"; +} } -namespace tf2 { - void PrintTo(const Quaternion& q, std::ostream* os) { - *os << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; - } +namespace tf2 +{ +void PrintTo(const Quaternion & q, std::ostream * os) +{ + *os << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; +} } // 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) +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) - ); + (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) @@ -1489,7 +1495,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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, sin(M_PI / 8), 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") || @@ -1504,7 +1512,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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, sin(-M_PI / 8), 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") || @@ -1518,7 +1528,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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, sin(M_PI / 4), 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") || @@ -1532,7 +1544,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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, sin(-M_PI / 4), 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") || @@ -1545,7 +1559,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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, sin(M_PI * 3 / 8), 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") || @@ -1558,7 +1574,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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, -sin(-M_PI * 3 / 8), -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") || @@ -1570,7 +1588,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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, sin(M_PI / 2), 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") || @@ -1582,7 +1602,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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, -sin(-M_PI / 2), 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") || @@ -1593,7 +1615,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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, sin(M_PI * 5 / 8), 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") || @@ -1604,7 +1628,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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, -sin(-M_PI * 5 / 8), -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") || @@ -1614,7 +1640,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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, -sin(M_PI * 6 / 8), -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,7 +1652,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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, -sin(-M_PI * 6 / 8), -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") || @@ -1633,7 +1663,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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, sin(-M_PI * 7 / 8), -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") || @@ -1642,7 +1674,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_PRED3(CheckQuaternionNear, outpose.transform.rotation, tf2::Quaternion(0, 0, sin(M_PI * 6 / 8), -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); } else { EXPECT_FALSE("Ring_45 testing Shouldn't get here"); printf( From 2e13e459e7c21f6bc9d17fa00b6ed11acbcc0dbb Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 22:03:28 -0800 Subject: [PATCH 27/44] uncrustify --- tf2/src/buffer_core.cpp | 33 +++++++++++++++++---------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 0d0d10f62..9887ddf75 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -183,14 +183,14 @@ bool BufferCore::setTransform( const std::string & authority, bool is_static) { tf2::Quaternion rotation( - transform.transform.rotation.x, - transform.transform.rotation.y, - transform.transform.rotation.z, - transform.transform.rotation.w); + transform.transform.rotation.x, + transform.transform.rotation.y, + transform.transform.rotation.z, + transform.transform.rotation.w); tf2::Vector3 origin( - transform.transform.translation.x, - transform.transform.translation.y, - transform.transform.translation.z); + 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( @@ -754,15 +754,15 @@ 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 +/** \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 + * \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. + * \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( @@ -772,22 +772,23 @@ void BufferCore::lookupTransformImpl( TimePoint & time_out) const { tf2::Quaternion rotation; - lookupTransformImpl(target_frame, source_frame, time, + 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 +/** \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. + * 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 + * 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 From 91935669de59e7b8625b1caa9b13176854addfe9 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 22:04:27 -0800 Subject: [PATCH 28/44] add helper to other file too --- .../test/test_tf2_geometry_msgs.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 452fc150d..1c9807c8a 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -67,6 +67,25 @@ geometry_msgs::msg::TransformStamped generate_stamped_transform() return t; } +// Helpers to print quaternions for tests +// 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 +{ +void PrintTo(const Quaternion & q, std::ostream * os) +{ + *os << "{" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << "}"; +} +} + +namespace tf2 +{ +void PrintTo(const Quaternion & q, std::ostream * os) +{ + *os << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; +} +} + // 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( From 66d5d81bb8219204cfa215979398545f3b29b256 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 4 Dec 2024 22:06:22 -0800 Subject: [PATCH 29/44] more comments, fix bad test data --- test_tf2/test/buffer_core_test.cpp | 4 ++-- tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index 078695459..da350e04e 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1371,7 +1371,7 @@ TEST(BufferCore_lookupTransform, multi_configuration) // } // -// Helpers to print quaternions for tests +// 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 @@ -1676,7 +1676,7 @@ TEST(BufferCore_lookupTransform, ring_45_configuration) EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon); EXPECT_PRED3( CheckQuaternionNear, outpose.transform.rotation, - tf2::Quaternion(0, 0, sin(M_PI * 6 / 8), -cos(M_PI * 6 / 8)), epsilon); + 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_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 1c9807c8a..a4765c9ef 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -67,7 +67,7 @@ geometry_msgs::msg::TransformStamped generate_stamped_transform() return t; } -// Helpers to print quaternions for tests +// 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 From 06c53689e2701c90dfd161aca52597e7b3fa709a Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Wed, 11 Dec 2024 15:19:50 -0800 Subject: [PATCH 30/44] Swap PrintTo with stream operator Co-authored-by: jmachowinski Signed-off-by: kyle-basis --- test_tf2/test/buffer_core_test.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index da350e04e..5a8d8b83d 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1382,9 +1382,7 @@ void PrintTo(const Quaternion & q, std::ostream * os) } } -namespace tf2 -{ -void PrintTo(const Quaternion & q, std::ostream * os) +std::ostream& operator<<(std::ostream& stream, const tf2::Quaternion & q) { *os << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; } From bdbbf811c5a20fc40343daabf9456177d3df912e Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Wed, 11 Dec 2024 15:19:59 -0800 Subject: [PATCH 31/44] Update tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp Co-authored-by: jmachowinski Signed-off-by: kyle-basis --- .../include/tf2_geometry_msgs/tf2_geometry_msgs.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 0687d9032..9662d669b 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,7 +853,7 @@ 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_); - tf2::Quaternion rotation = in.getRotation(); + 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(); From e117aa164f743c596255c9374492f11e71deb5b9 Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Wed, 11 Dec 2024 15:20:06 -0800 Subject: [PATCH 32/44] Update tf2/src/buffer_core.cpp Co-authored-by: jmachowinski Signed-off-by: kyle-basis --- tf2/src/buffer_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 9887ddf75..fca9702f1 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -187,7 +187,7 @@ bool BufferCore::setTransform( transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w); - tf2::Vector3 origin( + const tf2::Vector3 origin( transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z); From f9f7cea96ed4ae6520fa1c1aa045d90b22e4698f Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Wed, 11 Dec 2024 15:20:13 -0800 Subject: [PATCH 33/44] Update tf2/src/buffer_core.cpp Co-authored-by: jmachowinski Signed-off-by: kyle-basis --- tf2/src/buffer_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index fca9702f1..7011028ed 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -182,7 +182,7 @@ bool BufferCore::setTransform( const geometry_msgs::msg::TransformStamped & transform, const std::string & authority, bool is_static) { - tf2::Quaternion rotation( + const tf2::Quaternion rotation( transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, From c0f210899a80d9088fccb69e159c903fe5598256 Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Wed, 11 Dec 2024 15:36:58 -0800 Subject: [PATCH 34/44] Fix suggested change Signed-off-by: kyle-basis --- test_tf2/test/buffer_core_test.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index 5a8d8b83d..a05292faa 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1376,15 +1376,16 @@ TEST(BufferCore_lookupTransform, multi_configuration) // https://github.com/google/googletest/blob/main/docs/advanced.md#teaching-googletest-how-to-print-your-values namespace geometry_msgs::msg { -void PrintTo(const Quaternion & q, std::ostream * os) +std::ostream& operator<<(std::ostream& stream, const Quaternion & q) { - *os << "{" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << "}"; + stream << "{" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << "}"; } } -std::ostream& operator<<(std::ostream& stream, const tf2::Quaternion & q) +namespace tf2 { +std::ostream& operator<<(std::ostream& stream, const Quaternion & q) { - *os << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; + stream << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; } } From 71e0b94320b53ab6ffae7e875f28a62db417542e Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Wed, 11 Dec 2024 15:48:07 -0800 Subject: [PATCH 35/44] Update buffer_core_test.cpp Signed-off-by: kyle-basis --- test_tf2/test/buffer_core_test.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index a05292faa..a999a4648 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1379,6 +1379,7 @@ namespace geometry_msgs::msg std::ostream& operator<<(std::ostream& stream, const Quaternion & q) { stream << "{" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << "}"; + return stream; } } @@ -1386,6 +1387,7 @@ namespace tf2 { std::ostream& operator<<(std::ostream& stream, const Quaternion & q) { stream << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; + return stream; } } From ee7008f7d15d8e293ab4f7dbc93781926f639448 Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Wed, 11 Dec 2024 16:37:15 -0800 Subject: [PATCH 36/44] Update test_tf2_geometry_msgs.cpp Signed-off-by: kyle-basis --- tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index a4765c9ef..5a5269c06 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -72,17 +72,18 @@ geometry_msgs::msg::TransformStamped generate_stamped_transform() // https://github.com/google/googletest/blob/main/docs/advanced.md#teaching-googletest-how-to-print-your-values namespace geometry_msgs::msg { -void PrintTo(const Quaternion & q, std::ostream * os) +std::ostream& operator<<(std::ostream& stream, const Quaternion & q) { - *os << "{" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << "}"; + stream << "{" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << "}"; + return stream; } } -namespace tf2 +namespace tf2 { +std::ostream& operator<<(std::ostream& stream, const Quaternion & q) { -void PrintTo(const Quaternion & q, std::ostream * os) -{ - *os << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; + stream << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; + return stream; } } From 4b4e499859b219ce141776029e3c76aa7b73623d Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Mon, 16 Dec 2024 15:20:55 -0800 Subject: [PATCH 37/44] Update tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp Co-authored-by: Katherine Scott Signed-off-by: kyle-basis --- tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 5a5269c06..ff760c91f 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -80,7 +80,7 @@ std::ostream& operator<<(std::ostream& stream, const Quaternion & q) } namespace tf2 { -std::ostream& operator<<(std::ostream& stream, const Quaternion & q) +std::ostream & operator<<(std::ostream & stream, const Quaternion & q) { stream << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; return stream; From 546dc0e84ab28349da33936f8a9594387deba927 Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Mon, 16 Dec 2024 15:21:03 -0800 Subject: [PATCH 38/44] Update tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp Co-authored-by: Katherine Scott Signed-off-by: kyle-basis --- tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index ff760c91f..02d9bbf55 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -72,7 +72,7 @@ geometry_msgs::msg::TransformStamped generate_stamped_transform() // 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) +std::ostream & operator<<(std::ostream & stream, const Quaternion & q) { stream << "{" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << "}"; return stream; From ac1b45056e3cf177c359ac1d50c2091b6359634f Mon Sep 17 00:00:00 2001 From: kyle-basis Date: Mon, 16 Dec 2024 15:21:08 -0800 Subject: [PATCH 39/44] Update test_tf2/test/buffer_core_test.cpp Co-authored-by: jmachowinski Signed-off-by: kyle-basis --- test_tf2/test/buffer_core_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index a999a4648..f33d72ffb 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1413,7 +1413,7 @@ bool CheckQuaternionNear( TEST(BufferCore_lookupTransform, ring_45_configuration) { - double epsilon = 1e-6; + const double epsilon = 1e-6; rostest::Permuter permuter; std::vector times; From c0767e0a3fb29264e0c165a5ca7070ad323621e4 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Tue, 17 Dec 2024 16:45:43 -0800 Subject: [PATCH 40/44] uncrustify --- test_tf2/test/buffer_core_test.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index f33d72ffb..8482d913a 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -1376,15 +1376,16 @@ TEST(BufferCore_lookupTransform, multi_configuration) // 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) +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) +namespace tf2 +{ +std::ostream & operator<<(std::ostream & stream, const Quaternion & q) { stream << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; return stream; From acefb5ce60e66e8ea795d2bf5818652270c236e7 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 18 Dec 2024 14:30:18 -0800 Subject: [PATCH 41/44] attempt to fix merge conflict --- tf2/include/tf2/{buffer_core.h => buffer_core.hpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename tf2/include/tf2/{buffer_core.h => buffer_core.hpp} (100%) diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.hpp similarity index 100% rename from tf2/include/tf2/buffer_core.h rename to tf2/include/tf2/buffer_core.hpp From 61f82284b700ea980c3d821ba6b92f6e1fa7e471 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 18 Dec 2024 17:25:56 -0800 Subject: [PATCH 42/44] restore deleted file --- tf2/include/tf2/buffer_core.h | 45 +++++++++++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 tf2/include/tf2/buffer_core.h diff --git a/tf2/include/tf2/buffer_core.h b/tf2/include/tf2/buffer_core.h new file mode 100644 index 000000000..6ce942108 --- /dev/null +++ b/tf2/include/tf2/buffer_core.h @@ -0,0 +1,45 @@ +// Copyright 2008, Willow Garage, Inc. All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + + +/** \author Tully Foote */ + +#ifndef TF2__BUFFER_CORE_H_ +#define TF2__BUFFER_CORE_H_ + +# define BUFFER_CORE_HEADER_DEPERCATION This header is obsolete, \ + please include "tf2/buffer_core.hpp" instead + # ifdef _MSC_VER + # pragma message(BUFFER_CORE_HEADER_DEPERCATION) + # else + # warning BUFFER_CORE_HEADER_DEPERCATION + # endif + +#include + +#endif // TF2__BUFFER_CORE_H_ From 21883f52bab6db60b0f4413e2b484358109be365 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Wed, 18 Dec 2024 17:27:36 -0800 Subject: [PATCH 43/44] uncrustify --- tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp index 02d9bbf55..cdcdafdd5 100644 --- a/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp +++ b/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp @@ -79,7 +79,8 @@ std::ostream & operator<<(std::ostream & stream, const Quaternion & q) } } -namespace tf2 { +namespace tf2 +{ std::ostream & operator<<(std::ostream & stream, const Quaternion & q) { stream << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}"; From 47a5eda401355b70559a8ac5fcbe2a4cfdf77102 Mon Sep 17 00:00:00 2001 From: Kyle Franz Date: Thu, 19 Dec 2024 13:59:36 -0800 Subject: [PATCH 44/44] fix include guard --- tf2/include/tf2/buffer_core.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tf2/include/tf2/buffer_core.hpp b/tf2/include/tf2/buffer_core.hpp index 97c6e1bf3..0e363e5f6 100644 --- a/tf2/include/tf2/buffer_core.hpp +++ b/tf2/include/tf2/buffer_core.hpp @@ -29,8 +29,8 @@ /** \author Tully Foote */ -#ifndef TF2__BUFFER_CORE_H_ -#define TF2__BUFFER_CORE_H_ +#ifndef TF2__BUFFER_CORE_HPP_ +#define TF2__BUFFER_CORE_HPP_ #include #include @@ -468,4 +468,4 @@ class BufferCore : public BufferCoreInterface }; } // namespace tf2 -#endif // TF2__BUFFER_CORE_H_ +#endif // TF2__BUFFER_CORE_HPP_