Roslibrust tf support

[lucasw]

I made a version of this that works with https://github.com/Carter12s/roslibrust based on the rosrust version here.

There are a lot of changes made to the underlying tf system that may not have been necessary but I wanted to experiment:

chrono TimeDelta is used in place of the roslibrust Time in most places except for the Transform message because the latter doesn't have comparison or addition or subtraction support.

It was previously inverting every received transforms and adding those with parent-child reversed alongside the originals I think to avoid having to invert later in a lookup, but this made the tf tree harder to traverse to find the chain between any two frames. WIth a directed graph any two frames can traverse the tree upwards until they hit a root frame or a frame that the other traversal has already found, and then there is a tf chain (or disconnected trees if no frames in common).

Also the transform inversion on every incoming frame seemed unnecessary given that tf lookups are probably much lower frequency than incoming transforms, 10s of Hz vs. possibly hundreds of Hz, and there are many incoming transforms that are not part of the chain the node instantiating the tf listener cares about (it would be nice if those unneeded transforms could be dropped on reception, but that only works with a non-dynamic tree). There could be more improvements there where the found tf chain in a lookup knows exactly which transforms need inverting, currently it tries to find a given parent-child transform and fails and then looks up the reversed pair and does the inversion.

Loop detection is only done at lookup time, not for the entire received graph, which is less of a burden.

There are other changes made to handling static transforms and the tree dynamically changing that I made trying to debug lookup issues (which I may have created making other changes). I haven't looked at documentation or compared to behavior of the rospy/roscpp transform libraries, but I don't typically change parents of big parts of the tree or change a transform from static to non-static live, but I could see how some applications would want to do that. Rviz and other tools seem bad at handling big tree changes (maybe just those involving static tf changes?) and require restarts to flush old contradictory transforms, so I won't be surprised if it isn't handled well in the original C++ (or python).

I think the expiration of old transforms ought to be done across the entire buffer, the newest transform stamp - oldest transform stamp should be < cache duration for every parent-child, but currently individual transform pairs expire old transforms based on the time span of just that particular parent-child.

There's an echo.rs executable that mostly duplicates the output of tf2_tools echo.py but isn't yet doing any quaternion to euler conversions, and other command line options aren't supported.

It's up to the executable to update tf_listener subscribers alongside whatever subscribers and timed events they are doing, that could be improved. I wouldn't want to completely take control away by spawning threads behind the scenes, but maybe that should be optionally available.