LookupTransform error when using nav2 behavior tree

[stu00608]

Problem

• When I developing with our behavior tree similar to navigate_w_replanning_only_if_path_becomes_invalid.xml, the global plan only publish once, as long as the path is valid, it won't trigger computePathToPose.
• When over 5 secs, controller server throw out the error until goal failed.

  Extrapolation Error: Looup would require extrapolation into the past. Requested time 21.433000 but the earliest data is at time 27.733000. when looking up transform from frame [map] to frame [odom]
  

• I noticed that inside the error messages, reqested time is always the same. Which is the time of first path generated by global planner.
• In teb_local_planner_ros.cpp, lookup transform will take the stamp from global path. teb_local_planner_ros.cpp:724
• Is there any reason to always use global plan stamp as target time?
• I can solve the issue and run properly by change the target time to tf2::timeFromSec(0) (The second argument of lookupTransform)
  • 
• This video recorded the issue.

Conclusion

• So I'm wondering if this change can be added, since I'm not sure if it still fit every scenarios. Please let me know if there are something I missed in this issue.

Looking forwarfd to your reply, thanks!

[jcmonteiro]

I would say that tf2::TimePointZero is not the "correct" either. TEB will plan the path with respect to its knowledge of the world at a given time. In my opinion, it would make more sense to use the timestamp from the scan message. I say this because, at the end of the day, you are planning to avoid obstacles and your best guess of their poses comes from your previous measurement.

[stu00608]

I would say that tf2::TimePointZero is not the "correct" either. TEB will plan the path with respect to its knowledge of the world at a given time. In my opinion, it would make more sense to use the timestamp from the scan message. I say this because, at the end of the day, you are planning to avoid obstacles and your best guess of their poses comes from your previous measurement.

I agree with you, timestamp from scan message should not affect by behavior tree.
Also, I wanna know if the version that I modified whether use timeFromSec(0) or timestamp in latest scan message, is it gonna break the other parts inside TEB?

I think this should be fixed eventually with humble update. Since from nav2 humble release, they do have more example behavior trees that will cause this issue.

Thank you!

[jcmonteiro]

Also, I wanna know if the version that I modified whether use timeFromSec(0) or timestamp in latest scan message, is it gonna break the other parts inside TEB?

Not that I can think of right now since the overall behavior depends a lot on all the timestamps in your tf-tree.

By using the zero stamp you are allowing that particular lookup to consider the latest tfs in the tree, regardless of how old they are. The possible outcomes of doing so will be platform-specific.

[automech-rb]

It seems like ROS1 TEB uses ros::Time() instead of plan_pose.header.stamp in ROS2.

teb_local_planner/src/teb_local_planner_ros.cpp

Line 718 in 0e83907

geometry_msgs::TransformStamped plan_to_global_transform = tf.lookupTransform(global_frame, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp,

How about changing it similar to ROS1 method?

[stu00608]

There is a comment block seems doing the same thing in ros2_master branch.
Is there any reason to use plan_pose.header.stamp in newer version?

teb_local_planner/teb_local_planner/src/teb_local_planner_ros.cpp

Line 728 in 4501233

// tf_->waitForTransform(global_frame, ros::Time::now(),

[automech-rb]

This seems to be ported to crystal long time ago from this PR by @vinnamkim

Hey @vinnamkim , if possible can you explain your decision for changing the lookup time stamp?
Thanks