Remove many extra conversions from Matrix3x3 to Quaternion #741

kyle-basis · 2024-12-04T00:26:42Z

When calling setTransform(), rotation data would flow as ros_msgs::Quaternion -> tf2::Quaternion -> tf2::Matrix3x3, then reconvert to tf2::Quaternion 12 times to check validity of the rotation, then do one final conversion to actually store it inside the buffer.

When calling lookupTransform(), we'd go from tf2::Quaternion->tf2::Matrix3x3->tf2::Quaternion (x4)->ros_msgs::Quaternion.

I've fixed this in two parts -

Change setTransformImpl and lookupTransformImpl to take a vector+quat, eliminating both the roundtrip through tf2::Transfrom and the possible footgun of getRotation
In places that still need a tf2::Transform, only call getRotation() once, and reusing the result afterwards
I did not change the reference frame version of lookupTransform nor lookupVelocity, those are far less mechanical changes - would appreciate if someone fixed these up in a followup PR

Some notes:

getRotation() is poorly named - would recommend deprecating and renaming to calculateRotation or similar. I shot myself in the foot implementing tf2_basis and not realizing that origin is by reference and rotation is by value.
Part of the reason this happened is Draft: Break geometry_msgs dependency in tf2 #732 - there are conversion functions that actually do the conversion correctly (not converting once per component) but they are unable to be used due to circular reference
It feels like tf2::Transform should really be a vector+quat, but it would be an API change

tf2/include/tf2/buffer_core.h

kyle-basis · 2024-12-04T00:29:33Z

tf2/include/tf2/buffer_core.h

@@ -382,12 +382,15 @@ 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,


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.

tf2/src/buffer_core.cpp

kyle-basis · 2024-12-04T00:33:23Z

@kscottz - tagging as requested

kyle-basis · 2024-12-04T00:40:54Z

Note: I've broken all tests, I'll fix it - the base idea is sound, however

kyle-basis · 2024-12-04T00:47:42Z

Never mind - appears to just be tests using hardcoded quaternion values, rather than actually checking equality - I'm happy to fix this either by manually fixing the test quaternions, or by doing some nicer equality check. This does mean that downstream users will sometimes get out different (but equiv) rotations from the library than they got before.

kyle-basis · 2024-12-04T08:33:59Z

Pushed what should hopefully fix the failing tests. q == -q

test_tf2/test/buffer_core_test.cpp

tf2/src/buffer_core.cpp

Co-authored-by: jmachowinski <[email protected]> Signed-off-by: kyle-basis <[email protected]>

kyle-basis · 2024-12-04T21:41:28Z

Hit a failed test in CI, I don't think this is my fault - ci flake?

02:29:00 5:  - /usr/bin/python3 -m launch_testing.launch_test /tmp/ws/src/geometry2/test_tf2/test/buffer_client_tester.launch.py --junit-xml=/tmp/ws/test_results/test_tf2/test_buffer_client_tester.launch.py.xunit.xml --package-name=test_tf2
02:29:00 5: [INFO] [launch]: All log files can be found below /home/buildfarm/.ros/log/2024-12-04-02-29-00-675074-4d63d25031d7-4049
02:29:00 5: [INFO] [launch]: Default logging verbosity is set to INFO
02:29:00 5: test_termination (test_tf2.TestBufferClient.test_termination) ... [INFO] [static_transform_publisher-1]: process started with pid [4053]
02:29:00 5: [INFO] [test_buffer_server-2]: process started with pid [4054]
02:29:00 5: [INFO] [test_buffer_client-3]: process started with pid [4055]
02:29:00 5: [INFO] [python3-4]: process started with pid [4056]
02:29:00 5: [static_transform_publisher-1] [INFO] [1733308140.776567271] [static_transform_publisher_VNHQnJSRyXBSDNpK]: Spinning until stopped - publishing transform
02:29:00 5: [static_transform_publisher-1] translation: ('5.000000', '6.000000', '7.000000')
02:29:00 5: [static_transform_publisher-1] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
02:29:00 5: [static_transform_publisher-1] from 'a' to 'b'
02:29:00 5: [test_buffer_client-3] [==========] Running 2 tests from 1 test suite.
02:29:01 5: [test_buffer_client-3] [----------] Global test environment set-up.
02:29:01 5: [test_buffer_client-3] [----------] 2 tests from tf2_ros
02:29:01 5: [test_buffer_client-3] [ RUN      ] tf2_ros.buffer_client
02:29:01 5: [test_buffer_client-3] FOOO
02:29:01 5: [test_buffer_client-3] [INFO] [1733308142.718698560] [tf_action_node]: p1: (0.00, 0.00, 0.00), p2: (-5.00, -6.00, -7.00)
02:29:02 5: [test_buffer_client-3] [       OK ] tf2_ros.buffer_client (1014 ms)
02:29:02 5: [test_buffer_client-3] [ RUN      ] tf2_ros.buffer_client_different_types
02:29:02 5: [test_buffer_client-3] [ERROR] [1733308142.795791742] [tf_action_node]: Bullet: (-1.0000, 0.0000, 0.0000)
02:29:02 5: [test_buffer_client-3] [ERROR] [1733308142.795866165] [tf_action_node]: KDL: (0.0000, 0.0000, 0.0000)
02:29:02 5: [test_buffer_client-3] /tmp/ws/src/geometry2/test_tf2/test/test_buffer_client.cpp:127: Failure
02:29:02 5: [test_buffer_client-3] The difference between b1[0] and -5.0 is 4, which exceeds EPS, where
02:29:02 5: [test_buffer_client-3] b1[0] evaluates to -1,
02:29:02 5: [test_buffer_client-3] -5.0 evaluates to -5, and
02:29:02 5: [test_buffer_client-3] EPS evaluates to 0.001.
02:29:02 5: [test_buffer_client-3] 
02:29:02 5: [test_buffer_client-3] /tmp/ws/src/geometry2/test_tf2/test/test_buffer_client.cpp:128: Failure
02:29:02 5: [test_buffer_client-3] The difference between b1[1] and -6.0 is 6, which exceeds EPS, where
02:29:02 5: [test_buffer_client-3] b1[1] evaluates to 0,
02:29:02 5: [test_buffer_client-3] -6.0 evaluates to -6, and
02:29:02 5: [test_buffer_client-3] EPS evaluates to 0.001.
02:29:02 5: [test_buffer_client-3] 
02:29:02 5: [test_buffer_client-3] /tmp/ws/src/geometry2/test_tf2/test/test_buffer_client.cpp:129: Failure
02:29:02 5: [test_buffer_client-3] The difference between b1[2] and -7.0 is 7, which exceeds EPS, where
02:29:02 5: [test_buffer_client-3] b1[2] evaluates to 0,
02:29:02 5: [test_buffer_client-3] -7.0 evaluates to -7, and
02:29:02 5: [test_buffer_client-3] EPS evaluates to 0.001.
02:29:02 5: [test_buffer_client-3] 
02:29:02 5: [test_buffer_client-3] [  FAILED  ] tf2_ros.buffer_client_different_types (74 ms)
02:29:02 5: [test_buffer_client-3] [----------] 2 tests from tf2_ros (1089 ms total)
02:29:02 5: [test_buffer_client-3] 
02:29:02 5: [test_buffer_client-3] [----------] Global test environment tear-down
02:29:02 5: [test_buffer_client-3] [==========] 2 tests from 1 test suite ran. (1089 ms total)
02:29:02 5: [test_buffer_client-3] [  PASSED  ] 1 test.
02:29:02 5: [test_buffer_client-3] [  FAILED  ] 1 test, listed below:
02:29:02 5: [test_buffer_client-3] [  FAILED  ] tf2_ros.buffer_client_different_types
02:29:02 5: [test_buffer_client-3] 
02:29:02 5: [test_buffer_client-3]  1 FAILED TEST

Locally:

[test_buffer_client-3] [==========] Running 2 tests from 1 test suite.
[test_buffer_client-3] [----------] Global test environment set-up.
[test_buffer_client-3] [----------] 2 tests from tf2_ros
[test_buffer_client-3] [ RUN      ] tf2_ros.buffer_client
[test_buffer_client-3] FOOO
[test_buffer_client-3] [INFO] [1733348400.995224948] [tf_action_node]: p1: (0.00, 0.00, 0.00), p2: (-5.00, -6.00, -7.00)
[test_buffer_client-3] [       OK ] tf2_ros.buffer_client (403 ms)
[test_buffer_client-3] [ RUN      ] tf2_ros.buffer_client_different_types
[test_buffer_client-3] [ERROR] [1733348401.032240829] [tf_action_node]: Bullet: (-5.0000, -6.0000, -7.0000)
[test_buffer_client-3] [ERROR] [1733348401.032402204] [tf_action_node]: KDL: (0.0000, 0.0000, 0.0000)
[test_buffer_client-3] [       OK ] tf2_ros.buffer_client_different_types (30 ms)
[test_buffer_client-3] [----------] 2 tests from tf2_ros (434 ms total)
[test_buffer_client-3] 
[test_buffer_client-3] [----------] Global test environment tear-down
[test_buffer_client-3] [==========] 2 tests from 1 test suite ran. (434 ms total)
[test_buffer_client-3] [  PASSED  ] 2 tests.
[INFO] [test_buffer_client-3]: process has finished cleanly [pid 254386]
ok

kscottz · 2024-12-04T23:05:02Z

Sorry about the delay, had a power outage this morning. I'll take a look but I am not super familiar with the internals of TF / Geometry. Thanks for sticking with this and trying to get it over the line. We really appreciate the help and could always use a few more contributors. ❤️

Note: I've broken all tests, I'll fix it - the base idea is sound, however

I figured you might hit a few flakey tests as anything that relies on float / double ends up being a bit brittle. I can't speak to any of these tests being historically flakey. I think setting the epsilons to five or six sig figs should be sufficient.

This could probably use some documentation

Absolutely. We're starting to have conversations about bringing in someone to address API doc deficiencies but that's still far off. I would need more familiarity to do it myself.

Never mind - appears to just be tests using hardcoded quaternion values, rather than actually checking equality - I'm happy to fix this either by manually fixing the test quaternions, or by doing some nicer equality check. This does mean that downstream users will sometimes get out different (but equiv) rotations from the library than they got before.

This one's on you and what you're up for. I would have to look at the individual tests to make a call. Please do keep in mind that more you change on the lower level code the more likely it is that it may break something up stream. Smaller deltas are always better and get finished faster. You can always circle back

isNan() or isValid() on Vector/Quaternion would be really nice for this, along with a normalization check on quaternion - this code doesn't belong here.

I'll need to look at these a bit more but probably warrants an issue with a good first issue tag.

clalancette · 2024-12-04T23:11:28Z

I figured you might hit a few flakey tests as anything that relies on float / double ends up being a bit brittle. I can't speak to any of these tests being historically flakey. I think setting the epsilons to five or six sig figs should be sufficient.

For what it is worth, we have no reports of flakey tests from our nightly CI in this package that I can remember, and nothing is reported in https://github.com/ros2/geometry2/issues . So while it is always possible that there is a flakey test here, we do not currently know about any.

tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp

kscottz · 2024-12-04T23:37:09Z

tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp

-    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(


It might help the reviewers, and readability, if you articulate what's going on in this function call. I've been looking at this for ten minutes and it still isn't quite clear what's going on.

I added an intermediate variable - I think the diff moved your comment though, which line specifically is confusing?

Eh, that doesn't really tell me what the test does, it just tells me what the result should be. What would really help readers is something like, // Frame a and frame b differ by [x,y,z] and [p,q,r] at time t, check the resulting quat is <foo>

That's why I dislike this big ball of wax test pattern, when you write one function per test you can name the tests something sane like, "check_basic_frame_transform" and it all makes sense. The original author really should have at least left us a few bread crumb comments. 😢

Anyway, not your fault, thanks for putting up with this stuff.

tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp

kscottz

I think we're good to go once the uncrustify suggestions are merged.

kscottz · 2024-12-16T21:46:28Z

@kyle-basis I was busy last week but I wanted to check in on your progress here.

I added a couple of quick suggests to deal with uncrustify.

@kyle-basis I want to give you a shout out on the ROS channels. What would be really awesome to include is an estimate of much performance has improved with these changes. Would it be possible to time the test performance before and after these changes to see how things have improved?

Co-authored-by: Katherine Scott <[email protected]> Signed-off-by: kyle-basis <[email protected]>

Co-authored-by: jmachowinski <[email protected]> Signed-off-by: kyle-basis <[email protected]>

kyle-basis · 2024-12-16T23:22:46Z

Sorry, I've been busy with work on Basis and holiday stuff! Merged the suggestions. I can try and get performance numbers for before and after. It's tricky as I don't actually have a full ROS environment to test some of this stuff in.

ahcorde · 2024-12-17T08:55:30Z

tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp

+}
+}
+
+namespace tf2 {


Suggested change

namespace tf2 {

namespace tf2

{

uncrustify https://build.ros2.org/job/Rpr__geometry2__ubuntu_noble_amd64/240/

jmachowinski · 2024-12-17T17:53:53Z

@kyle-basis
ament_uncrustify --reformat
is your friend here ;-)

tfoote · 2024-12-18T00:11:37Z

getRotation() is poorly named - would recommend deprecating and renaming to calculateRotation or similar. I shot myself in the foot implementing tf2_basis and not realizing that origin is by reference and rotation is by value.

It feels like tf2::Transform should really be a vector+quat, but it would be an API change

Both of these are legacy from the LInearMath library that we have forked from Bullet to use internally: https://github.com/bulletphysics/bullet3/tree/master/src/LinearMath

We chose to internalize this library with renaming process to break the dependency and it's main goal is to only be internally used. But we didn't restrict access and so people have used it despite our preference for it not to be used directly. We've tried to keep this to be a direct fork instead of evolving it and improving it. We don't want to get into the business of maintaining yet another Linear Math library. The recommendation is to use Eigen or any other one of your own choice instead, leveraging the templated interface to tf2.

kyle-basis · 2024-12-18T00:41:57Z

That's reasonable - still, the hard dependency on geometry_msgs is painful and unnecessary.

kyle-basis · 2024-12-18T00:45:31Z

@kyle-basis ament_uncrustify --reformat is your friend here ;-)

I've used this before, it ends up reformatting a lot of files in tf2/, but will try and run it more precisely.

ahcorde · 2024-12-18T08:51:47Z

Pulls: #741
Gist: https://gist.githubusercontent.com/ahcorde/f91deb8bb6b91dd854769ff2ccc660c1/raw/a373fa10f8335099af08f044cb888a9555482834/ros2.repos
BUILD args: --packages-above-and-dependencies test_tf2 tf2 tf2_geometry_msgs --packages-above-and-dependencies test_tf2 tf2 tf2_geometry_msgs
TEST args: --packages-above test_tf2 tf2 tf2_geometry_msgs --packages-above test_tf2 tf2 tf2_geometry_msgs
ROS Distro: rolling
Job: ci_launcher
ci_launcher ran: https://ci.ros2.org/job/ci_launcher/14977

Linux
Linux-aarch64
Linux-rhel
Windows

ahcorde

I merged the another PR and now we have conflicts. Sorry about that, do you mind to fix it ?

kyle-basis · 2024-12-18T22:28:35Z

Let me see what I can do.

kyle-basis · 2024-12-18T22:32:52Z

Manually moving buffer_core.h before the merge appears to have cleaned up the conflict, we'll see if CI is happy.

ahcorde

do you mind to restore tf2/include/tf2/buffer_core.h with https://github.com/ros2/geometry2/blob/rolling/tf2/include/tf2/buffer_core.h ?

kyle-basis · 2024-12-19T01:26:18Z

Sorry about that - done

ahcorde

cpplint is failing https://build.ros2.org/job/Rpr__geometry2__ubuntu_noble_amd64/244/testReport/junit/(root)/projectroot/cpplint/

kyle-basis · 2024-12-19T21:56:53Z

No problems found??

kyle-basis · 2024-12-19T22:00:03Z

Fixed. Will work on perf testing today or tomorrow.

kyle-basis · 2024-12-19T23:02:16Z

Will fix the BUFFER_CORE_INTERFACE_HEADER_DEPERCATION warnings, too.

...should I make a PR to fix the spelling of that warning?

Edit: these warnings don't appear to be my fault.

kyle-basis added 7 commits December 3, 2024 15:28

fix repeated calls to Transform::getRotation

246fa84

pick up one more place

e3ccee1

add new setTransformImpl

e791331

change setTransformImpl to take origin and rotation

51f75b4

remove deleted code

32660ca

fix missing reference

46d66fc

add vector+quat interface to lookupTransformImpl

0361624

kyle-basis commented Dec 4, 2024

View reviewed changes

tf2/include/tf2/buffer_core.h Outdated Show resolved Hide resolved

kyle-basis commented Dec 4, 2024

View reviewed changes

tf2/src/buffer_core.cpp Show resolved Hide resolved

fix tabs

a979245

kyle-basis added 2 commits December 4, 2024 00:11

fix tests, I hope

7e0f4b3

uncrustify

dc2f979

jmachowinski suggested changes Dec 4, 2024

View reviewed changes

kyle-basis and others added 5 commits December 4, 2024 02:07

fix extraneous lines

0030dfb

add comment, std abs

d524ed8

remove unused transform

8bd813a

Update tf2/src/buffer_core.cpp

77c8010

Co-authored-by: jmachowinski <[email protected]> Signed-off-by: kyle-basis <[email protected]>

fix another test

b399f8f

yet another failing test

bc5db14

kscottz reviewed Dec 4, 2024

View reviewed changes

tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp Show resolved Hide resolved

kscottz reviewed Dec 4, 2024

View reviewed changes

tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp Show resolved Hide resolved

kscottz reviewed Dec 4, 2024

View reviewed changes

kscottz reviewed Dec 16, 2024

View reviewed changes

tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp Outdated Show resolved Hide resolved

kscottz reviewed Dec 16, 2024

View reviewed changes

tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp Outdated Show resolved Hide resolved

kscottz approved these changes Dec 16, 2024

View reviewed changes

kyle-basis and others added 3 commits December 16, 2024 15:20

Update tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp

4b4e499

Co-authored-by: Katherine Scott <[email protected]> Signed-off-by: kyle-basis <[email protected]>

Update tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp

546dc0e

Co-authored-by: Katherine Scott <[email protected]> Signed-off-by: kyle-basis <[email protected]>

Update test_tf2/test/buffer_core_test.cpp

ac1b450

Co-authored-by: jmachowinski <[email protected]> Signed-off-by: kyle-basis <[email protected]>

ahcorde requested changes Dec 17, 2024

View reviewed changes

uncrustify

c0767e0

ahcorde approved these changes Dec 18, 2024

View reviewed changes

ahcorde requested changes Dec 18, 2024

View reviewed changes

kyle-basis added 2 commits December 18, 2024 14:30

attempt to fix merge conflict

acefb5c

Merge https://github.com/ros2/geometry2 into fix_get_rotation_calls

b24d3dc

ahcorde requested changes Dec 18, 2024

View reviewed changes

restore deleted file

61f8228

uncrustify

21883f5

ahcorde requested changes Dec 19, 2024

View reviewed changes

fix include guard

47a5eda

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Remove many extra conversions from Matrix3x3 to Quaternion #741

Remove many extra conversions from Matrix3x3 to Quaternion #741

kyle-basis commented Dec 4, 2024

kyle-basis Dec 4, 2024 •

edited

Loading

kyle-basis commented Dec 4, 2024

kyle-basis commented Dec 4, 2024

kyle-basis commented Dec 4, 2024

kyle-basis commented Dec 4, 2024

kyle-basis commented Dec 4, 2024

kscottz commented Dec 4, 2024

clalancette commented Dec 4, 2024

kscottz Dec 4, 2024

kyle-basis Dec 5, 2024

kscottz Dec 6, 2024

kscottz left a comment

kscottz commented Dec 16, 2024

kyle-basis commented Dec 16, 2024

ahcorde Dec 17, 2024

ahcorde Dec 17, 2024

kscottz Dec 17, 2024

jmachowinski commented Dec 17, 2024

tfoote commented Dec 18, 2024

kyle-basis commented Dec 18, 2024

kyle-basis commented Dec 18, 2024

ahcorde commented Dec 18, 2024

ahcorde left a comment

kyle-basis commented Dec 18, 2024

kyle-basis commented Dec 18, 2024

ahcorde left a comment

kyle-basis commented Dec 19, 2024

ahcorde left a comment

kyle-basis commented Dec 19, 2024

kyle-basis commented Dec 19, 2024

kyle-basis commented Dec 19, 2024 •

edited

Loading

Remove many extra conversions from Matrix3x3 to Quaternion #741

Are you sure you want to change the base?

Remove many extra conversions from Matrix3x3 to Quaternion #741

Conversation

kyle-basis commented Dec 4, 2024

kyle-basis Dec 4, 2024 • edited Loading

Choose a reason for hiding this comment

kyle-basis commented Dec 4, 2024

kyle-basis commented Dec 4, 2024

kyle-basis commented Dec 4, 2024

kyle-basis commented Dec 4, 2024

kyle-basis commented Dec 4, 2024

kscottz commented Dec 4, 2024

clalancette commented Dec 4, 2024

kscottz Dec 4, 2024

Choose a reason for hiding this comment

kyle-basis Dec 5, 2024

Choose a reason for hiding this comment

kscottz Dec 6, 2024

Choose a reason for hiding this comment

kscottz left a comment

Choose a reason for hiding this comment

kscottz commented Dec 16, 2024

kyle-basis commented Dec 16, 2024

ahcorde Dec 17, 2024

Choose a reason for hiding this comment

ahcorde Dec 17, 2024

Choose a reason for hiding this comment

kscottz Dec 17, 2024

Choose a reason for hiding this comment

jmachowinski commented Dec 17, 2024

tfoote commented Dec 18, 2024

kyle-basis commented Dec 18, 2024

kyle-basis commented Dec 18, 2024

ahcorde commented Dec 18, 2024

ahcorde left a comment

Choose a reason for hiding this comment

kyle-basis commented Dec 18, 2024

kyle-basis commented Dec 18, 2024

ahcorde left a comment

Choose a reason for hiding this comment

kyle-basis commented Dec 19, 2024

ahcorde left a comment

Choose a reason for hiding this comment

kyle-basis commented Dec 19, 2024

kyle-basis commented Dec 19, 2024

kyle-basis commented Dec 19, 2024 • edited Loading

kyle-basis Dec 4, 2024 •

edited

Loading

kyle-basis commented Dec 19, 2024 •

edited

Loading