search

ros / geometry2

A set of ROS packages for keeping track of coordinate transforms.
189 stars 273 forks source link

Fail out of lookupTransform immediately if extrapolation into the past is required instead of waiting for timeout #524

Open lucasw opened 2 years ago

lucasw commented 2 years ago

At any time except startup where the tf buffer is unpopulated (where tf lookups are likely to fail anyhow) there's no reason to expect that an into the past extrapolation error will start working by waiting even longer (and it gets even further into the past). Certainly if the lookup time is beyond the cache time of the buffer in the past it will never succeed by waiting. (Time going backwards like from looping rosbags is handled mostly elsewhere, I'd have to double check that)

I've just started looking at the code but I think this could be accomplished by splitting tf2_msgs::TF2Error::EXTRAPOLATION_ERROR into a future and past error, and modification/complication of the canTransform or timeout logic in checkTransform() and goalCB (could consolidate what looks like identical code in those two at the same time, or in an earlier commit).

https://github.com/ros/geometry2/blob/c73b5939723db078c9bbe18523230ad54f859682/tf2_ros/src/buffer_server.cpp#L141-L158

Maybe it would be canTransform(gh) || oldTransform(gh) || goal_info.end_time <= ros::Time::now()) where oldTransform() could be a new method to only look for past extrapolation errors. I'll try something like that out unless I hear any better suggestions.

(I only bumped into this issue because the gazebo camera sensor and maybe other gazebo plugins are publishing while paused, which I don't think was happening a year ago, so there are bunch of messages filling up queues of other nodes that are respecting the pause so coming out of pause they're stuck waiting for timeouts on a bunch of old messages... but it seems like even then they should get caught up sooner than I'm observing- could subscriber queues behave funny while paused?)

tfoote commented 2 years ago

This does generally make sense as an efficiency improvement, and avoiding more polling. However I would suggest targeting this towards https://github.com/ros2/geometry2 instead of here which is basically end of life.