Bug: Occasional NaN transforms

[bluenote10]

We noticed that the transforms return from tf/tf2 occasionally contain NaN values in the translation part. Some debugging revealed that this is caused by the interpolation logic of the time cache. The bug is hidden here:

geometry2/tf2/src/cache.cpp

Lines 178 to 185 in 0761810

	// Check for zero distance case
	if( two.stamp_ == one.stamp_ )
	{
	output = two;
	return;
	}
	//Calculate the ratio
	tf2Scalar ratio = (time.toSec() - one.stamp_.toSec()) / (two.stamp_.toSec() - one.stamp_.toSec());

The equality check compares the timestamps based on sec/nsec integers, however the ratio converts them to double. In our case the two timestamps are only a few nanoseconds apart, and the resolution after the toSec() conversion is not enough. Thus there is a division by zero and the interpolation returns meaningless results due to ratio being NaN.

I will open a PR with the most obvious fix: Checking that the toSec() difference is non-zero instead of the stamp equality check.