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

[lucasw]

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).

geometry2/tf2_ros/src/buffer_server.cpp

Lines 141 to 158 in c73b593

	GoalInfo goal_info;
	goal_info.handle = gh;
	goal_info.end_time = ros::Time::now() + gh.getGoal()->timeout;
	//we can do a quick check here to see if the transform is valid
	//we'll also do this if the end time has been reached
	if(canTransform(gh) || goal_info.end_time <= ros::Time::now())
	{
	tf2_msgs::LookupTransformResult result;
	try
	{
	result.transform = lookupTransform(gh);
	}
	catch (tf2::ConnectivityException &ex)
	{
	result.error.error = result.error.CONNECTIVITY_ERROR;
	result.error.error_string = ex.what();
	}

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]

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.