Interpolate function in cache.cpp seems to output wrong timestamp

Next Generation ROS
1

I have been trying to troubleshoot why the TF is not working on our platform. While reading through the code, I saw that the interpolate function in “geometry2/tf2/src/cache.cpp” seems to be outputting the wrong timestamp. The code is below:

void TimeCache::interpolate(const TransformStorage& one, const TransformStorage& two, TimePoint time, TransformStorage& output)
{
// Check for zero distance case
if( two.stamp_ == one.stamp_ )
{
output = two;
return;
}
//Calculate the ratio
tf2Scalar ratio = double((time - one.stamp_).count()) / double((two.stamp_ - one.stamp_).count());

//Interpolate translation
output.translation_.setInterpolate3(one.translation_, two.translation_, ratio);

//Interpolate rotation
output.rotation_ = slerp( one.rotation_, two.rotation_, ratio);

output.stamp_ = one.stamp_;
output.frame_id_ = one.frame_id_;
output.child_frame_id_ = one.child_frame_id_;
}

Shouldn’t output.stamp_ be the time that is queried for? I seem to be receiving correct data overall, so I am thinking maybe this output for the timestamp is ignored higher up, but I was curious of why it is this way.

2

Yes, that looks like a bug. And it should set the output to the query time. It looks to be an issue upstream. I suspect you’re right it’s generally masked due to small interpolation values from high frequency inputs and or higher level APIs ignoring that return value and assuming the query times. And the low level unit tests don’t assert the timestamp with the output value. A bug or PR for that would be appreciated.

3

I submitted a bug for that. It seems like I do not have permissions to make a remote branch in order to do a PR.

4

You need to fork the repo with your github user and then create a branch there. Then you’ll be able to send a PR to merge your fork branch into the upstream main/devel one.