[ros-users] [Discourse.ros.org] [Next Generation ROS] Interpolate function in cache.cpp seems to output wrong timestamp

[vsherrod]

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.





---
[Visit Topic](https://discourse.ros.org/t/interpolate-function-in-cache-cpp-seems-to-output-wrong-timestamp/2489/1) or reply to this email to respond.