doTransform non stamped msgs

[adamkrawczyk]

Response to:
#337

I didn't implement toMsg and fromMsg for PoseWithCovariance because there is no tf2 equivalent to geometry_msgs PoseWithCovariance

[aprotyas]

The PR is much larger than it has to be because you moved functions around. What was the rationale behind that? Reverting those changes would make reviewing more straightforward too.

[adamkrawczyk]

I've applied your suggestion, hope it's fine now 😄

[aprotyas]

The doTransform methods introduced for the non-stamped message types look mostly fine to me. Two concerns though:

• There's still a lot of refactoring that adds noise to this PR. You should defer structural refactoring for a different PR. If you're fine with it, I can force push a commit that removes any unnecessary moving-around of the older methods. Please check 2050824 to see what I mean. That's the commit with least noise.
• I think unit tests for some of the methods are missing.

[adamkrawczyk]

Okay, you are right. Do apply your path and then I will open next PR with refactor I've done before and finally we do new unit tests. I was wondering whether to add new tests here but most of existing are time related tests also the issues was not about this.

To sum up if you agree with me please close the PR and do think about better unit tests.

Thank you for your patience and help :)

[aprotyas]

Alright, I've updated the header file in question to retain your changes but introduce less noise (74c7d82).

unit tests for some of the methods are missing

By this, I meant that you did not introduce unit tests for the doTransform specializations for the non-stamped message types. I've introduced said tests in 48a18fb. Unfortunately, after much talk about the PR size, these new tests make this PR huge... 🤷

please close the PR and do think about better unit tests.

I don't think this PR needs to be closed; but yes, unit tests (not those that target the changes introduced in this PR) is part of a conversation outside of this PR's scope.

---

By the way, having tested locally, doTransform seems to be failing for the Pose, PoseWithCovariance, Point, and Transform message types - consistently getting the value for the x position wrong. It would be great if you had any insight into why that was happening. For your reference, you can find the test failures in this build job:

[adamkrawczyk]

Okay, I found the issue behind this build fails. Let me explain it:

I did manual calculations

1. Convert quaternion to rotation matrix

                     [ 1 , 0, 0]
quat(1,0,0,0) ->     [ 0, -1, 0]
                     [ 0, 0, -1]

2. Convert to 4x4 rotation-translation matrix

                        [ 1 , 0, 0, 1]
mat  ->                 [ 0, -1, 0, 2]
                        [ 0, 0, -1, 3]
                        [ 0, 0, 0, 1 ]

                        [ 1 , 0, 0, 10]
mat2  ->                [ 0, -1, 0, 20]
                        [ 0, 0, -1, 30]
                        [ 0, 0, 0, 1  ]

3. Perform matrix multiplication mat2 * mat

                  [ 1 , 0, 0, 11]
mat2 * mat =      [ 0, -1, 0, 18]
                  [ 0, 0, -1, 27]
                  [ 0, 0, 0, 1  ]

Which solves the issue. I will prepare a path for this - simple change val and also will add the same tests for other msg types (stamped) which fails too.

I'm 99% sure about this but as always check me.

Best

[aprotyas]

also will add the same tests for other msg types (stamped) which fails too.

I don't think this is necessary to add because the tf_buffer->transform invocation uses the doTransform methods anyway. Which is why I'm curious because the doTransform itself returns x==11 whereas tf_buffer->transform returns x==-9. Maybe I'm misunderstanding the math here...

[adamkrawczyk]

I've changed return val to 11. Also to prove that implementation is right I have applied orginal tests from geometry_msgs on other branch more-test-branch. Try them locally, all tests pass.

[aprotyas]

I think the changes in d67d92f are appropriate, but I am still perplexed by how the doTransform perform the transform operation with the exact transformation specified (from frame A to frame B), but tf_buffer->transform seems to do so with the inverse transformation (from frame B to frame A).

@tfoote would you be able to shed some light about why - for the tests in tf2_geometry_msgs - the straightforward doTransform returns a vector {11, 18, 27} while the tf_buffer->transform returns a vector {-9, 18, 27} (the inverse transformation, essentially)? I digged through the code a little bit and it looks like lookupTransform returns the inverse transformation to what's specified in the tests, but I might be misunderstanding transformations and the parent-child relation here.

FYI: the transform specified in the tests. Note that lookupTransform(target=B, source=A, ...) returns the inverse of this transformation.

geometry2/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp

Lines 292 to 302 in 8b12844

	geometry_msgs::msg::TransformStamped t;
	t.transform.translation.x = 10;
	t.transform.translation.y = 20;
	t.transform.translation.z = 30;
	t.transform.rotation.w = 0;
	t.transform.rotation.x = 1;
	t.transform.rotation.y = 0;
	t.transform.rotation.z = 0;
	t.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2));
	t.header.frame_id = "A";
	t.child_frame_id = "B";

[adamkrawczyk]

Any updates, @tfoote ?

[tfoote]

Yes, that's a common confusion. If you are asking for the transform from frame A to frame B it's the inverse of the transform which will take data in frame A and express it in frame B. That's why we use target and source frame nomenclature for where the data is coming from and going to. It starts in the source frame and ends in the target frame. And no one externally needs to know about the structure or order of the transforms except the implementors.

[adamkrawczyk]

Thank you for your response. Can it be merged now? @aprotyas

[adamkrawczyk]

@tfoote Can you merge and close this PR?

[clalancette]

Generally looks fine, just two small nits to take care of. Once those are done I'll run CI on the whole thing.

[clalancette]

CI:

• Linux
• Linux-aarch64
• macOS
• Windows
• Windows Debug

[clalancette]

All right, CI is all green here. Thanks for the contribution, merging.