Skip to content

Remove many extra conversions from Matrix3x3 to Quaternion #741

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

Merged
ahcorde merged 48 commits into from
Jan 14, 2025
Merged

Remove many extra conversions from Matrix3x3 to Quaternion #741

Show file tree
Hide file tree
Changes from 10 commits
Commits
Show all changes
48 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
9d145fb
Merge https://github.com/ros2/geometry2 into fix_get_rotation_calls
kyle-basis Dec 27, 2024
1ecb27e
undo changes to header includes
kyle-basis Dec 27, 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
68 changes: 30 additions & 38 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,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)
);
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
}

TEST(BufferCore_lookupTransform, ring_45_configuration)
{
double epsilon = 1e-6;
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
Expand Down Expand Up @@ -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);
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
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 +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") ||
Expand All @@ -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") ||
Expand All @@ -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") ||
Expand All @@ -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") ||
Expand All @@ -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") ||
Expand All @@ -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);
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
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 @@ -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);
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
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 @@ -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);
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
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 @@ -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);
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
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 @@ -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);
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
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 @@ -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);
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
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 @@ -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);
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
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 @@ -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);
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
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(
Expand Down
10 changes: 7 additions & 3 deletions tf2/include/tf2/buffer_core.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -382,12 +382,16 @@ class BufferCore : public BufferCoreInterface
std::string allFramesAsStringNoLock() const;

bool setTransformImpl(
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
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
93 changes: 54 additions & 39 deletions tf2/src/buffer_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -182,27 +182,27 @@ bool BufferCore::setTransform(
const geometry_msgs::msg::TransformStamped & transform,
const std::string & authority, bool is_static)
{
tf2::Transform tf2_transform(tf2::Quaternion(
tf2::Quaternion rotation(
Copy link
Copy Markdown
Contributor

@kscottz kscottz Dec 5, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Note for the next reviewer, this change keeps the rotation quat as a quat and the positions as a Vec3 instead of promoting it to a tf2::Transform, saving us a slight conversion cost.

Comment thread
kyle-basis marked this conversation as resolved.
Outdated
transform.transform.rotation.x,
transform.transform.rotation.y,
transform.transform.rotation.z,
transform.transform.rotation.w),
tf2::Vector3(
transform.transform.rotation.w);
tf2::Vector3 origin(
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
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::nanoseconds>(
std::chrono::seconds(
transform.header.stamp.sec)));
return setTransformImpl(
tf2_transform, transform.header.frame_id, transform.child_frame_id,
origin, rotation, transform.header.frame_id, transform.child_frame_id,
Copy link
Copy Markdown
Contributor

@kscottz kscottz Dec 5, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Note to next reviewer: now we just called the new overloaded function.

time_point, authority, is_static);
}

bool BufferCore::setTransformImpl(
const tf2::Transform & transform_in, const std::string frame_id,
const std::string child_frame_id, const TimePoint stamp,
const tf2::Vector3 & origin_in, const tf2::Quaternion & rotation_in, const std::string & frame_id,
Copy link
Copy Markdown
Contributor

@kscottz kscottz Dec 5, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Since we're doing some heavy construction here we would wise to add some documentation for the next person who has to work on this code.

const std::string & child_frame_id, const TimePoint stamp,
const std::string & authority, bool is_static)
{
std::string stripped_frame_id = stripSlash(frame_id);
Expand Down Expand Up @@ -231,36 +231,36 @@ bool BufferCore::setTransformImpl(
error_exists = true;
}

if (std::isnan(transform_in.getOrigin().x()) || std::isnan(transform_in.getOrigin().y()) ||
std::isnan(transform_in.getOrigin().z()) ||
std::isnan(transform_in.getRotation().x()) || std::isnan(transform_in.getRotation().y()) ||
std::isnan(transform_in.getRotation().z()) || std::isnan(transform_in.getRotation().w()))
if (std::isnan(origin_in.x()) || std::isnan(origin_in.y()) ||
std::isnan(origin_in.z()) ||
std::isnan(rotation_in.x()) || std::isnan(rotation_in.y()) ||
std::isnan(rotation_in.z()) || std::isnan(rotation_in.w()))
{
RCUTILS_LOG_ERROR(
"TF_NAN_INPUT: Ignoring transform for child_frame_id \"%s\" from authority \"%s\" because"
" of a nan value in the transform (%f %f %f) (%f %f %f %f)",
stripped_child_frame_id.c_str(), authority.c_str(),
transform_in.getOrigin().x(), transform_in.getOrigin().y(), transform_in.getOrigin().z(),
transform_in.getRotation().x(), transform_in.getRotation().y(),
transform_in.getRotation().z(), transform_in.getRotation().w()
origin_in.x(), origin_in.y(), origin_in.z(),
Comment thread
kscottz marked this conversation as resolved.
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) {
RCUTILS_LOG_ERROR(
"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;
}

Expand All @@ -287,8 +287,8 @@ bool BufferCore::setTransformImpl(

if (frame->insertData(
TransformStorage(
stamp, transform_in.getRotation(),
transform_in.getOrigin(), lookupOrInsertFrameNumber(stripped_frame_id), frame_number)))
stamp, rotation_in,
origin_in, lookupOrInsertFrameNumber(stripped_frame_id), frame_number)))
{
frame_authority_[frame_number] = authority;
} else {
Expand Down Expand Up @@ -700,15 +700,17 @@ BufferCore::lookupTransform(
{
tf2::Transform transform;
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
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<std::chrono::nanoseconds>(
time_out.time_since_epoch());
std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(
Expand Down Expand Up @@ -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();
Comment thread
kyle-basis marked this conversation as resolved.
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<std::chrono::nanoseconds>(
time_out.time_since_epoch());
std::chrono::seconds s = std::chrono::duration_cast<std::chrono::seconds>(
Expand All @@ -755,13 +758,25 @@ BufferCore::lookupTransform(
void BufferCore::lookupTransformImpl(
Comment thread
kyle-basis marked this conversation as resolved.
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);
Comment thread
kyle-basis marked this conversation as resolved.
Outdated
transform_out.setRotation(rotation);
}

void BufferCore::lookupTransformImpl(
Comment thread
kyle-basis marked this conversation as resolved.
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<std::mutex> 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);
Expand Down Expand Up @@ -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(
Expand Down
Loading