Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Remove many extra conversions from Matrix3x3 to Quaternion #741

Open
wants to merge 46 commits into
base: rolling
Choose a base branch
from
+257 −152
Open

Remove many extra conversions from Matrix3x3 to Quaternion #741

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
46 commits
Select commit Hold shift + click to select a range
246fa84
fix repeated calls to Transform::getRotation
kyle-basis Dec 3, 2024
e3ccee1
pick up one more place
kyle-basis Dec 3, 2024
e791331
add new setTransformImpl
kyle-basis Dec 3, 2024
51f75b4
change setTransformImpl to take origin and rotation
kyle-basis Dec 3, 2024
32660ca
remove deleted code
kyle-basis Dec 3, 2024
46d66fc
fix missing reference
kyle-basis Dec 3, 2024
0361624
add vector+quat interface to lookupTransformImpl
kyle-basis Dec 4, 2024
a979245
fix tabs
kyle-basis Dec 4, 2024
7e0f4b3
fix tests, I hope
kyle-basis Dec 4, 2024
dc2f979
uncrustify
kyle-basis Dec 4, 2024
0030dfb
fix extraneous lines
kyle-basis Dec 4, 2024
d524ed8
add comment, std abs
kyle-basis Dec 4, 2024
8bd813a
remove unused transform
kyle-basis Dec 4, 2024
77c8010
Update tf2/src/buffer_core.cpp
kyle-basis Dec 4, 2024
b399f8f
fix another test
kyle-basis Dec 4, 2024
bc5db14
yet another failing test
kyle-basis Dec 4, 2024
be1ba27
Update tf2/src/buffer_core.cpp
kyle-basis Dec 5, 2024
86c6181
Update tf2/src/buffer_core.cpp
kyle-basis Dec 5, 2024
bb368e5
Update tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp
kyle-basis Dec 5, 2024
a2e08e6
Update tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp
kyle-basis Dec 5, 2024
5d33825
Apply suggestions from code review
kyle-basis Dec 5, 2024
edb14fe
one more const
kyle-basis Dec 5, 2024
7c93938
Merge remote-tracking branch 'origin/fix_get_rotation_calls' into fix…
kyle-basis Dec 5, 2024
ca99cf9
readibility
kyle-basis Dec 5, 2024
095195c
uncrustify
kyle-basis Dec 5, 2024
eaa507b
add helpers to print
kyle-basis Dec 5, 2024
6774cb6
uncrustify
kyle-basis Dec 5, 2024
2e13e45
uncrustify
kyle-basis Dec 5, 2024
9193566
add helper to other file too
kyle-basis Dec 5, 2024
66d5d81
more comments, fix bad test data
kyle-basis Dec 5, 2024
06c5368
Swap PrintTo with stream operator
kyle-basis Dec 11, 2024
bdbbf81
Update tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp
kyle-basis Dec 11, 2024
e117aa1
Update tf2/src/buffer_core.cpp
kyle-basis Dec 11, 2024
f9f7cea
Update tf2/src/buffer_core.cpp
kyle-basis Dec 11, 2024
c0f2108
Fix suggested change
kyle-basis Dec 11, 2024
71e0b94
Update buffer_core_test.cpp
kyle-basis Dec 11, 2024
ee7008f
Update test_tf2_geometry_msgs.cpp
kyle-basis Dec 12, 2024
4b4e499
Update tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp
kyle-basis Dec 16, 2024
546dc0e
Update tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp
kyle-basis Dec 16, 2024
ac1b450
Update test_tf2/test/buffer_core_test.cpp
kyle-basis Dec 16, 2024
c0767e0
uncrustify
kyle-basis Dec 18, 2024
acefb5c
attempt to fix merge conflict
kyle-basis Dec 18, 2024
b24d3dc
Merge https://github.com/ros2/geometry2 into fix_get_rotation_calls
kyle-basis Dec 18, 2024
61f8228
restore deleted file
kyle-basis Dec 19, 2024
21883f5
uncrustify
kyle-basis Dec 19, 2024
47a5eda
fix include guard
kyle-basis Dec 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
141 changes: 84 additions & 57 deletions test_tf2/test/buffer_core_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1371,9 +1371,50 @@ TEST(BufferCore_lookupTransform, multi_configuration)
// }
//

// Helpers used by gtest to print quaternions when errors happen
// Must be defined in the namespace the class to be printed in lives, as per
// https://github.com/google/googletest/blob/main/docs/advanced.md#teaching-googletest-how-to-print-your-values
namespace geometry_msgs::msg
{
std::ostream & operator<<(std::ostream & stream, const Quaternion & q)
{
stream << "{" << q.x << ", " << q.y << ", " << q.z << ", " << q.w << "}";
return stream;
}
}

namespace tf2
{
std::ostream & operator<<(std::ostream & stream, const Quaternion & q)
{
stream << "{" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "}";
return stream;
}
}

// Runs an equality check between a and b, allowing for the valid case that a == -b
// https://gamedev.stackexchange.com/questions/75072/how-can-i-compare-two-quaternions-for-logical-equality/75077#75077
bool CheckQuaternionNear(
const geometry_msgs::msg::Quaternion & a, const tf2::Quaternion & b,
double epsilon)
{
return (
(std::abs(a.x - b.x()) < epsilon) &&
(std::abs(a.y - b.y()) < epsilon) &&
(std::abs(a.z - b.z()) < epsilon) &&
(std::abs(a.w - b.w()) < epsilon)
) ||
(
(std::abs(a.x + b.x()) < epsilon) &&
(std::abs(a.y + b.y()) < epsilon) &&
(std::abs(a.z + b.z()) < epsilon) &&
(std::abs(a.w + b.w()) < epsilon)
);
}

TEST(BufferCore_lookupTransform, ring_45_configuration)
{
double epsilon = 1e-6;
const double epsilon = 1e-6;
rostest::Permuter permuter;

std::vector<builtin_interfaces::msg::Time> times;
Expand Down Expand Up @@ -1456,10 +1497,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration)
EXPECT_NEAR(outpose.transform.translation.x, sqrt(2) / 2 - 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, sqrt(2) / 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI / 8), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI / 8), epsilon);
EXPECT_PRED3(
CheckQuaternionNear, outpose.transform.rotation,
tf2::Quaternion(0, 0, sin(M_PI / 8), cos(M_PI / 8)), epsilon);
}
// Inverse Chaining 1
else if ((source_frame == "b" && target_frame == "a") ||
Expand All @@ -1474,10 +1514,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration)
EXPECT_NEAR(outpose.transform.translation.x, sqrt(2) / 2 - 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2) / 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI / 8), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI / 8), epsilon);
EXPECT_PRED3(
CheckQuaternionNear, outpose.transform.rotation,
tf2::Quaternion(0, 0, sin(-M_PI / 8), cos(-M_PI / 8)), epsilon);
}
// Chaining 2
else if ((source_frame == "a" && target_frame == "c") ||
Expand All @@ -1491,10 +1530,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration)
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI / 4), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI / 4), epsilon);
EXPECT_PRED3(
CheckQuaternionNear, outpose.transform.rotation,
tf2::Quaternion(0, 0, sin(M_PI / 4), cos(M_PI / 4)), epsilon);
}
// Inverse Chaining 2
else if ((source_frame == "c" && target_frame == "a") ||
Expand All @@ -1508,10 +1546,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration)
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI / 4), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI / 4), epsilon);
EXPECT_PRED3(
CheckQuaternionNear, outpose.transform.rotation,
tf2::Quaternion(0, 0, sin(-M_PI / 4), cos(-M_PI / 4)), epsilon);
}
// Chaining 3
else if ((source_frame == "a" && target_frame == "d") ||
Expand All @@ -1524,10 +1561,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration)
EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2) / 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, sqrt(2) / 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI * 3 / 8), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI * 3 / 8), epsilon);
EXPECT_PRED3(
CheckQuaternionNear, outpose.transform.rotation,
tf2::Quaternion(0, 0, sin(M_PI * 3 / 8), cos(M_PI * 3 / 8)), epsilon);
}
// Inverse Chaining 3
else if ((target_frame == "a" && source_frame == "d") ||
Expand All @@ -1540,10 +1576,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration)
EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2) / 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2) / 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.z, -sin(-M_PI * 3 / 8), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, -cos(-M_PI * 3 / 8), epsilon);
EXPECT_PRED3(
CheckQuaternionNear, outpose.transform.rotation,
tf2::Quaternion(0, 0, -sin(-M_PI * 3 / 8), -cos(-M_PI * 3 / 8)), epsilon);
}
// Chaining 4
else if ((source_frame == "a" && target_frame == "e") ||
Expand All @@ -1555,10 +1590,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration)
EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI / 2), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI / 2), epsilon);
EXPECT_PRED3(
CheckQuaternionNear, outpose.transform.rotation,
tf2::Quaternion(0, 0, sin(M_PI / 2), cos(M_PI / 2)), epsilon);
}
// Inverse Chaining 4
else if ((target_frame == "a" && source_frame == "e") ||
Expand All @@ -1570,10 +1604,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration)
EXPECT_NEAR(outpose.transform.translation.x, -2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.z, -sin(-M_PI / 2), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, cos(-M_PI / 2), epsilon);
EXPECT_PRED3(
CheckQuaternionNear, outpose.transform.rotation,
tf2::Quaternion(0, 0, -sin(-M_PI / 2), cos(-M_PI / 2)), epsilon);
}
// Chaining 5
else if ((source_frame == "a" && target_frame == "f") ||
Expand All @@ -1584,10 +1617,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration)
EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2) / 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2) / 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI * 5 / 8), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, cos(M_PI * 5 / 8), epsilon);
EXPECT_PRED3(
CheckQuaternionNear, outpose.transform.rotation,
tf2::Quaternion(0, 0, sin(M_PI * 5 / 8), cos(M_PI * 5 / 8)), epsilon);
}
// Inverse Chaining 5
else if ((target_frame == "a" && source_frame == "f") ||
Expand All @@ -1598,10 +1630,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration)
EXPECT_NEAR(outpose.transform.translation.x, -1 - sqrt(2) / 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, sqrt(2) / 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.z, -sin(-M_PI * 5 / 8), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, -cos(-M_PI * 5 / 8), epsilon);
EXPECT_PRED3(
CheckQuaternionNear, outpose.transform.rotation,
tf2::Quaternion(0, 0, -sin(-M_PI * 5 / 8), -cos(-M_PI * 5 / 8)), epsilon);
}
// Chaining 6
else if ((source_frame == "a" && target_frame == "g") ||
Expand All @@ -1611,10 +1642,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration)
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.z, -sin(M_PI * 6 / 8), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, -cos(M_PI * 6 / 8), epsilon);
EXPECT_PRED3(
CheckQuaternionNear, outpose.transform.rotation,
tf2::Quaternion(0, 0, -sin(M_PI * 6 / 8), -cos(M_PI * 6 / 8)), epsilon);
}
// Inverse Chaining 6
else if ((target_frame == "a" && source_frame == "g") ||
Expand All @@ -1624,10 +1654,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration)
EXPECT_NEAR(outpose.transform.translation.x, -1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.z, -sin(-M_PI * 6 / 8), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, -cos(-M_PI * 6 / 8), epsilon);
EXPECT_PRED3(
CheckQuaternionNear, outpose.transform.rotation,
tf2::Quaternion(0, 0, -sin(-M_PI * 6 / 8), -cos(-M_PI * 6 / 8)), epsilon);
}
// Chaining 7
else if ((source_frame == "a" && target_frame == "h") ||
Expand All @@ -1636,10 +1665,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration)
EXPECT_NEAR(outpose.transform.translation.x, sqrt(2) / 2 - 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, -sqrt(2) / 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.z, sin(-M_PI * 7 / 8), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, -cos(-M_PI * 7 / 8), epsilon);
EXPECT_PRED3(
CheckQuaternionNear, outpose.transform.rotation,
tf2::Quaternion(0, 0, sin(-M_PI * 7 / 8), -cos(-M_PI * 7 / 8)), epsilon);
}
// Inverse Chaining 7
else if ((target_frame == "a" && source_frame == "h") ||
Expand All @@ -1648,10 +1676,9 @@ TEST(BufferCore_lookupTransform, ring_45_configuration)
EXPECT_NEAR(outpose.transform.translation.x, sqrt(2) / 2 - 1, epsilon);
EXPECT_NEAR(outpose.transform.translation.y, sqrt(2) / 2, epsilon);
EXPECT_NEAR(outpose.transform.translation.z, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.x, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.y, 0, epsilon);
EXPECT_NEAR(outpose.transform.rotation.z, sin(M_PI * 7 / 8), epsilon);
EXPECT_NEAR(outpose.transform.rotation.w, -cos(M_PI * 7 / 8), epsilon);
EXPECT_PRED3(
CheckQuaternionNear, outpose.transform.rotation,
tf2::Quaternion(0, 0, sin(M_PI * 7 / 8), -cos(M_PI * 7 / 8)), epsilon);
} else {
EXPECT_FALSE("Ring_45 testing Shouldn't get here");
printf(
Expand Down
18 changes: 11 additions & 7 deletions tf2/include/tf2/buffer_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,13 +44,13 @@
#include <utility>
#include <vector>

#include "LinearMath/Transform.hpp"
#include "LinearMath/Transform.h"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/velocity_stamped.hpp"
#include "rcutils/logging_macros.h"
#include "tf2/buffer_core_interface.hpp"
#include "tf2/exceptions.hpp"
#include "tf2/transform_storage.hpp"
#include "tf2/buffer_core_interface.h"
#include "tf2/exceptions.h"
#include "tf2/transform_storage.h"
#include "tf2/visibility_control.h"

namespace tf2
Expand Down Expand Up @@ -382,12 +382,16 @@ class BufferCore : public BufferCoreInterface
std::string allFramesAsStringNoLock() const;

bool setTransformImpl(
const tf2::Transform & transform_in, const std::string frame_id,
const std::string child_frame_id, const TimePoint stamp,
const tf2::Vector3 & origin_in, const tf2::Quaternion & rotation_in,
const std::string & frame_id, const std::string & child_frame_id, const TimePoint stamp,
const std::string & authority, bool is_static);
void lookupTransformImpl(
const std::string & target_frame, const std::string & source_frame,
const TimePoint & time_in, tf2::Transform & transform, TimePoint & time_out) const;
const TimePoint & time_in, tf2::Transform & transform_out, TimePoint & time_out) const;
void lookupTransformImpl(
const std::string & target_frame, const std::string & source_frame,
const TimePoint & time_in, tf2::Vector3 & origin_out, tf2::Quaternion & rotation_out,
TimePoint & time_out) const;

void lookupTransformImpl(
const std::string & target_frame, const TimePoint & target_time,
Expand Down
Loading