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
Open

Remove many extra conversions from Matrix3x3 to Quaternion #741

Show file tree
Hide file tree
Changes from 7 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
9 changes: 6 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,15 @@ class BufferCore : public BufferCoreInterface
std::string allFramesAsStringNoLock() const;

bool setTransformImpl(
kyle-basis marked this conversation as resolved.
Show resolved Hide resolved
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
Author

@kyle-basis kyle-basis Dec 4, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

note: I changed the string values into references - this really should be a std::string_view - it shouldn't break any code, and ROS is c++17 now - but it's somewhat a moot point as SSO will kick in for most char* conversions.

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

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.

kyle-basis marked this conversation as resolved.
Show resolved Hide resolved
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(
kyle-basis marked this conversation as resolved.
Show resolved Hide resolved
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

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

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(),
kscottz marked this conversation as resolved.
Show resolved Hide 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;
kyle-basis marked this conversation as resolved.
Show resolved Hide resolved
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();
kyle-basis marked this conversation as resolved.
Show resolved Hide 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(
kyle-basis marked this conversation as resolved.
Show resolved Hide 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);
kyle-basis marked this conversation as resolved.
Show resolved Hide resolved
transform_out.setRotation(rotation);
}

void BufferCore::lookupTransformImpl(
kyle-basis marked this conversation as resolved.
Show resolved Hide 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
9 changes: 5 additions & 4 deletions tf2_geometry_msgs/include/tf2_geometry_msgs/tf2_geometry_msgs.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
kyle-basis marked this conversation as resolved.
Show resolved Hide resolved
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();
Expand Down