Unexpected output of _chainAsVector()

[JohannesErnst]

Bug report / Missing functionality

Required Info:

• Operating System:
  • OpenSUSE 15.5
• Installation type:
  • binaries
• Version or commit hash:
  • Humble
• DDS implementation:
  • N/A
• Client library (if applicable):
  • rclpy

Explanation

In tf2 for ros1 there existed a function to retrieve the chain of two frames as a list of strings. Now migrating to ros2, we need the functionality of this function. The corresponding function of tf2 for ros2 is here:

geometry2/tf2/src/buffer_core.cpp

Lines 1549 to 1631 in c3cafef

	void BufferCore::_chainAsVector(
	const std::string & target_frame, TimePoint target_time,
	const std::string & source_frame, TimePoint source_time,
	const std::string & fixed_frame,
	std::vector<std::string> & output) const
	{
	std::string error_string;
	output.clear(); // empty vector
	std::unique_lock<std::mutex> lock(frame_mutex_);
	TransformAccum accum;
	// Get source frame/time using getFrame
	CompactFrameID source_id = lookupFrameNumber(source_frame);
	CompactFrameID fixed_id = lookupFrameNumber(fixed_frame);
	CompactFrameID target_id = lookupFrameNumber(target_frame);
	std::vector<CompactFrameID> source_frame_chain;
	tf2::TF2Error retval = walkToTopParent(
	accum, source_time, fixed_id, source_id, &error_string,
	&source_frame_chain);
	if (retval != tf2::TF2Error::TF2_NO_ERROR) {
	switch (retval) {
	case tf2::TF2Error::TF2_CONNECTIVITY_ERROR:
	throw ConnectivityException(error_string);
	case tf2::TF2Error::TF2_EXTRAPOLATION_ERROR:
	throw ExtrapolationException(error_string);
	case tf2::TF2Error::TF2_LOOKUP_ERROR:
	throw LookupException(error_string);
	default:
	CONSOLE_BRIDGE_logError("Unknown error code: %d", retval);
	assert(0);
	}
	}
	if (source_time != target_time) {
	std::vector<CompactFrameID> target_frame_chain;
	retval = walkToTopParent(
	accum, target_time, target_id, fixed_id, &error_string,
	&target_frame_chain);
	if (retval != tf2::TF2Error::TF2_NO_ERROR) {
	switch (retval) {
	case tf2::TF2Error::TF2_CONNECTIVITY_ERROR:
	throw ConnectivityException(error_string);
	case tf2::TF2Error::TF2_EXTRAPOLATION_ERROR:
	throw ExtrapolationException(error_string);
	case tf2::TF2Error::TF2_LOOKUP_ERROR:
	throw LookupException(error_string);
	default:
	CONSOLE_BRIDGE_logError("Unknown error code: %d", retval);
	assert(0);
	}
	}
	size_t m = target_frame_chain.size();
	size_t n = source_frame_chain.size();
	while (m > 0u && n > 0u) {
	--m;
	--n;
	if (source_frame_chain[n] != target_frame_chain[m]) {
	break;
	}
	}
	// Erase all duplicate items from frame_chain
	if (n > 0u) {
	source_frame_chain.erase(source_frame_chain.begin() + (n - 1u), source_frame_chain.end());
	}
	if (m < target_frame_chain.size()) {
	for (size_t i = 0u; i <= m; ++i) {
	source_frame_chain.push_back(target_frame_chain[i]);
	}
	}
	}
	// Write each element of source_frame_chain as string
	for (size_t i = 0u; i < source_frame_chain.size(); ++i) {
	output.push_back(lookupFrameString(source_frame_chain[i]));
	}
	}
	} // namespace tf2

However, it is now not wrapped as a listener function but can only be accessed via the private member function buffer._chain(). The bigger problem is that the implementation seems erroneous. Consider this simple tf tree:

   A
 /   \
B     D
|
C

Querying _chainAsVector(target_frame=D, source_frame=C, fixed_frame=A) (regardless of the time), yields the chain ['C', 'B'] and not as expected ['C', 'B', 'A', 'D'].

Does anyone have an idea why it is like this?

And also why the implementation changed from ros1 tf2 to ros2 tf2 (compare upper links) while the previous implementation worked just fine?

Is there a similar function that I don't know of that can achieve the same thing (python)?

[Sukhvansh2004]

Hi, can I work on issue?

[JohannesErnst]

Would be great! Taking a little closer look, the algorithm here does not make a lot of sense to me. At least this line seems fishy:

geometry2/tf2/src/buffer_core.cpp

Line 1586 in c3cafef

if (source_time != target_time) {

And also this here doesn't seem right to me, how can you find a common parent like this if source_frame_chain is walked source_frame to fixed_frame and target_frame_chain is fixed_frame to target_frame. To my understanding the target_frame_chain then should be inverted to be target_frame to fixed_frame . But I didn't test it out with actual values...

geometry2/tf2/src/buffer_core.cpp

Lines 1607 to 1613 in c3cafef

	while (m > 0u && n > 0u) {
	--m;
	--n;
	if (source_frame_chain[n] != target_frame_chain[m]) {
	break;
	}
	}

I recommend rather implementing the way it was done in tf2 for ros1, see here:
https://github.com/ros/geometry2/blob/6df9e94363061a24ba890a6ff29771a96b0d756b/tf2/src/buffer_core.cpp#L1583-L1654

[Sukhvansh2004]

So I have made a PR regarding the issues I though on. You can check it out once and lemme know if you think there are some more issues which needs to be addressed. I am also trying to make a test file for this function so as to be sure of its functionality

[JohannesErnst]

I think what you added seems reasonable to me but I don't think it is enough. As mentioned above, this line does not make sense to me currently:

geometry2/tf2/src/buffer_core.cpp

Line 1586 in c3cafef

if (source_time != target_time) {

Right now, we will only reach the lines of code you edited if the source_time != target_time. What is the reason for that? In my current test example, I am using the same source_time and target_time. That means the function will just output the result from source_frame_chain:

geometry2/tf2/src/buffer_core.cpp

Lines 1568 to 1571 in c3cafef

	std::vector<CompactFrameID> source_frame_chain;
	tf2::TF2Error retval = walkToTopParent(
	accum, source_time, fixed_id, source_id, &error_string,
	&source_frame_chain);

But the result from source_frame_chain which is the chain from fixed_frame to source_frame seems already wrong. If I query this on the first example for fixed_frame=A and source_frame=D, I do not get [D, A, B, C] but just [C].

   A
 /   \
B     D
|
C

So maybe the chain returned from walkToTopParent is already wrong? We have a similar logic there, see here:

geometry2/tf2/src/buffer_core.cpp

Lines 469 to 492 in c3cafef

	if (frame_chain) {
	// Pruning: Compare the chains starting at the parent (end) until they differ
	size_t m = reverse_frame_chain.size();
	size_t n = frame_chain->size();
	while (m > 0u && n > 0u) {
	--m;
	--n;
	if ((*frame_chain)[n] != reverse_frame_chain[m]) {
	break;
	}
	}
	// Erase all duplicate items from frame_chain
	if (n > 0u) {
	frame_chain->erase(frame_chain->begin() + (n - 1u), frame_chain->end());
	}
	if (m < reverse_frame_chain.size()) {
	size_t i = m + 1uL;
	while (i > 0u) {
	--i;
	frame_chain->push_back(reverse_frame_chain[i]);
	}
	}
	}

So I guess we have to apply similar changes here first?

[Sukhvansh2004]

Yeah I think you are right, I'll just check on this. Also can you share the code you're using to debug and check this out so as I can also run and check this at my end too

[JohannesErnst]

Unfortunately, we don't really have a debug environment. The setup we are using is quite complex, in the end we use the python wrapper in tf2_py and query buffer._chain but there are quite some layers in between that I cannot share. I did the tests for the tree above with a different complex tree but just used simple letters for demonstration. The rest we deducted from looking at the code. I think if the (n - 1u) is switched to (n + 1u) in walkToTopParent as well, this it should do the trick but proper testing is definitely needed... Sorry that I cannot help with the debug env anymore

[Sukhvansh2004]

So I have fixed a few things now. First in the chainAsVector function the target_id and fixed_id were I think wrongly given for the target_chain so I just swapped and fixed that. Second while adding the target chain back into the source chain we would need to include the last common node too so as to form the connecting link of the chains, so I have added and fixed on that too (similiar fix is done in the walkToTopParent function too.

[JohannesErnst]

Amazing! I think that should do the trick but I think we cannot really test it until it is properly released. Thanks for the work!

[Sukhvansh2004]

Can you tag a maintainer of this repo who you know so that he can review the PR once and see if more changes are required or not and if not then merge it.

[JohannesErnst]

I am not in contact or know any maintainers myself, but looking into the contributors list, maybe @tfoote can take care of this or tag some other maintainer?
This is the pr regarding this issue: #750