Don't insert a TF frame if one of the same timestamp already exists, instead just overwrite it. #426
Conversation
…instead just overwrite it.
pbeeson
changed the title
tf2/src/cache.cpp
Outdated
| @@ -261,7 +261,10 @@ bool TimeCache::insertData(const TransformStorage& new_data) | |||
| break; | |||
| storage_it++; | |||
| } | |||
| storage_.insert(storage_it, new_data); | |||
| if (storage_it != storage_.end() && storage_it->stamp_ == new_data.stamp_) | |||
| *storage_it = new_data; | |||
There was a problem hiding this comment.
As discussed in #414 I'd rather see the newer data dropped instead of overwriting the older data. With the current semantics /tf data is effectively considered immutable. And without the ability to rewrite sections of data consistently you could easily end up with data from two datasources being improperly interpolated between.
There was a problem hiding this comment.
OK. I changed the patch to return immediately when a duplicate timestamp is seen instead of overwriting data.
There was a problem hiding this comment.
In simply changing the assignment to use the old value, one of the tests in test_tf2 was failing. Further analysis showed that it was not anchoring the forward and inverse transforms to the first frame in the list but the second one.
So, the new code for rejecting redundant transforms actually caught a bug in the tests. 🍰
…nd normal transform was frame 'b' instead of frame 'a', thus creating a problem
|
Great thanks for the update it looks great. I'd like to add one more element which is to add a warning that the message is being dropped. Also I'd like to check if we should be returning false from the insertData call if we don't insert. As I think that makes sense since the data wasn't inserted like the other false return case. Maybe the error can be caught one layer higher, but likely we'll have to just print a warning to console. |
|
The issue with returning. False is that the level higher just says that the
data is old. I didn’t want to change the function too much, but we could
return an error to be printed when false is returned, but then this level
doesn’t have all the variables that are printed for the existing message
(if I recall correctly).
|
|
OK @tfoote. I pushed a change that does accomplish both an error message AND returns false. Because there are 2 ways false can happen now, and there are 2 different error messages, the easiest was to borrow what other functions do here and pass back an error_message to the higher level. |
|
This pattern looks good for the long term with passing the, but at first pass it looks like adding the new argument is an ABI breaking change that I'd like to avoid backporting to the existing distros to avoid breaking people. I can setup a new |
|
That seems fair to me. I’d prefer the previous fix (where we silently
overwrite and return true) for backporting, and the current API change for
future releases.
|
|
Merged in #459 |
|
We ran into the same problem... |
In ROS Noetic the behavior has changed and transform updates are ignored if the timestamp is the same, see ros/geometry2#426 (comment) This breaks the update logic in the previous approach
* [tf2_geometry_msgs] pep8 formatting * [tf2_geometry_msgs] fix copyright * tf2_geometry_msgs header guard and copyright * More flake8 stuff Co-authored-by: Bjar Ne <gleichdick@users.noreply.github.com>
So, we ran into an issue with ROS on a project (still on kinetic due to legacy robot issues, but this applies to melodic too, so here's a melodic PR).
Generally, if a node is broadcasting TF data based on other incoming frames and/or other information, and something goes wrong, it will stop publishing data. But we ran into a scenario where something went wrong in a node, and it kept broadcasting the same TF data with the same timestamp, forever. When this happened, we noticed that all nodes with a tf listener started immediately consuming more and more memory.
In looking into this, it is clear that when storing an incoming TF in the tf2 listener code, that if a frame between parent and child exists with the same timestamp, that the incoming data is not ignored, nor does it overwrite the previous frame, but it gets inserted next to the existing frame with the same timestamp, so that now there are multiple transforms between the same two frames with the same timestamp.
I added some debug console outputs and a small publisher node and a node that just makes a tf listener and spins. This proved that the storage for tf listener does indeed grow infinitely so long as no newer data comes in that is more than max_duration (10 seconds) to force these to be pruned.
The fix is VERY simple, and has been tested with the stand alone programs. where previously the size of storage_ grew without bound when sending the exact same message in a loop, but now remains size 1 for that parent/child transform.