Fix issue #749

[Sukhvansh2004]

So what I saw in the function implementation was that the indexing in the erase function was not correct and the target chain not pushed correctly back into the source chain, so have fixed that.

Fixes #749

[Sukhvansh2004]

Hi @clalancette ,

I noticed you are an active maintainer of this repository. Would you mind reviewing this PR? If everything looks good, it would be great if you could merge it into the repo.

Thanks for your time and help!

[tfoote]

Making a core change like this will need to have a test case that we can review for correctness. And then from that we can consider the correctness of the existing implementation and the new proposed one.

How did you validate the indexing shifts?
And why did you remove the time check?

[tfoote]

Removing this check is clearly wrong. You only traverse the tree a second time if the times are different.

[Sukhvansh2004]

In the Noetic implementation ig this was'nt there as mentioned in the issue which is addressed by this PR

[Yadunund]

@Sukhvansh2004 could you please follow up on @tfoote's feedback?

[Sukhvansh2004]

Hi, I have been a bit busy with some work. Can I come back to you guys next week?

[tfoote]

Sure, no problem

[ryosakagami]

Thank you @Sukhvansh2004 for working on the fixes of the _chainAsVector. I have been having the same issue that the returned tf chain is not correct, and had a closer look in to this PR by myself.

When I tested this PR with my system, the returned tf chain does not seem to be always correct. I am not precisely sure which specific part of this PR is not yet correct, though.

Nevertheless, I tried my best to understand the current code and made changes by myself to find a working solution. I hope this helps for further development.
Here is the change I tested by myself: ryosakagami@4124c33

I see the issue comes from two different parts: walkToTopParent and _chainAsVector.

walkToTopParent

What I realized is that there are several lines of codes related to frame_chain was removed in the walkToTopParent function, compared to ROS1 noetic. This seems to be the core cause of the issue.

1. reversing frame_chain when the source_frame is a direct parent of the target_frame
   In ROS1 noetic, the frame_chain was reversed:
   https://github.com/ros/geometry2/blob/9ff608f463d98f27b99d575fa89000a498ed2d4f/tf2/src/buffer_core.cpp#L431-L437
   In ROS2 humble, this part of logic is no longer there:
   

   geometry2/tf2/src/buffer_core.cpp

   Lines 432 to 434 in 1417e78

   	if (frame_chain) {
   	frame_chain->swap(reverse_frame_chain);
   	}

2. adding top_parent to frame_chain after walking from target_frame to top_parent
   In ROS1 noetic, top_parent is added to frame_chain:
   https://github.com/ros/geometry2/blob/9ff608f463d98f27b99d575fa89000a498ed2d4f/tf2/src/buffer_core.cpp#L477-L483
   In ROS2 humble, this also does not exist:
   

   geometry2/tf2/src/buffer_core.cpp

   Lines 466 to 468 in 1417e78

   	}
   	f.finalize(FullPath, time);

3. index adaptation
   In ROS1 noetic, n + 2 was used:
   https://github.com/ros/geometry2/blob/9ff608f463d98f27b99d575fa89000a498ed2d4f/tf2/src/buffer_core.cpp#L500
   In ROS2 humble, as you noticed, n - 1 is used:
   

   geometry2/tf2/src/buffer_core.cpp

   Line 482 in 1417e78

   frame_chain->erase(frame_chain->begin() + (n - 1u), frame_chain->end());

_chainAsVector

After introducing these fixes in walkToTopParent, I simplified _chainAsVector to directly utilize walkToTopParent from source_frame to target_frame
The purpose was to see if the changes above to the walkToTopParent are reasonable.
The parameters source_time and fixed_frame are ignored for now.
Since walkToTopParent does actually walk from source_frame to target_frame already, I suspected that frame chain could be acquired in this way also.
This returns reasonable tf frame chains in my system.

However, I am not exactly sure if this simplified version suffices the requirements, since I don't understand why fixed_frame and two different time stamps are passed as arguments to this function.