From 9fd7da10698d89fa72b6f44ecd85483432713d81 Mon Sep 17 00:00:00 2001 From: Patrick Beeson Date: Fri, 18 Oct 2019 20:33:04 -0500 Subject: [PATCH 1/9] Don't insert a TF frame is one of the same timestamp already exists, instead just overwrite it. --- tf2/src/cache.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index edbbc7633..2affdc5c2 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -261,7 +261,10 @@ bool TimeCache::insertData(const TransformStorage& new_data) break; storage_it++; } - storage_.insert(storage_it, new_data); + if (storage_it->stamp_ == new_data.stamp_) + *storage_it = new_data; + else + storage_.insert(storage_it, new_data); pruneList(); return true; From 1f7edbd25714f9be530094984f43deb2034edef5 Mon Sep 17 00:00:00 2001 From: Patrick Beeson Date: Fri, 18 Oct 2019 21:21:53 -0500 Subject: [PATCH 2/9] Fix case where storage_ starts empty --- tf2/src/cache.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index 2affdc5c2..cb5c91e76 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -261,7 +261,7 @@ bool TimeCache::insertData(const TransformStorage& new_data) break; storage_it++; } - if (storage_it->stamp_ == new_data.stamp_) + if (storage_it != storage_.end() && storage_it->stamp_ == new_data.stamp_) *storage_it = new_data; else storage_.insert(storage_it, new_data); From 754d93d658e64c71c7874d32cd978e06e12d1de7 Mon Sep 17 00:00:00 2001 From: Patrick Beeson Date: Mon, 21 Oct 2019 21:37:29 -0500 Subject: [PATCH 3/9] Don't overwrite an existing TF, use the existing one --- tf2/src/cache.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index cb5c91e76..72252722c 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -262,9 +262,13 @@ bool TimeCache::insertData(const TransformStorage& new_data) storage_it++; } if (storage_it != storage_.end() && storage_it->stamp_ == new_data.stamp_) - *storage_it = new_data; + { + return true; + } else + { storage_.insert(storage_it, new_data); + } pruneList(); return true; From 21bd33f36d3fcb8dd81efefe2bba7db903b07968 Mon Sep 17 00:00:00 2001 From: Patrick Beeson Date: Mon, 21 Oct 2019 23:06:01 -0500 Subject: [PATCH 4/9] Fix to improper ring_45 test, where 'anchor' frame for both inverse and normal transform was frame 'b' instead of frame 'a', thus creating a problem --- test_tf2/test/buffer_core_test.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index fd15d67d2..f1f9e7516 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -213,11 +213,12 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const ros::Time & else ts.header.stamp = ros::Time(); - ts.header.frame_id = frame_prefix + frames[i-1]; + ts.child_frame_id = frame_prefix + frames[i]; if (i > 1) - ts.child_frame_id = frame_prefix + frames[i]; - else - ts.child_frame_id = frames[i]; // connect first frame + ts.header.frame_id = frame_prefix + frames[i-1]; + else + ts.header.frame_id = frames[i-1]; + EXPECT_TRUE(mBC.setTransform(ts, "authority")); if (interpolation_space > ros::Duration()) { From abc38168393bf12e415a3883846b1195a62e08db Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Tue, 22 Oct 2019 16:29:09 -0700 Subject: [PATCH 5/9] removing tabs --- test_tf2/test/buffer_core_test.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/test_tf2/test/buffer_core_test.cpp b/test_tf2/test/buffer_core_test.cpp index f1f9e7516..4a869e5f1 100644 --- a/test_tf2/test/buffer_core_test.cpp +++ b/test_tf2/test/buffer_core_test.cpp @@ -213,11 +213,11 @@ void setupTree(tf2::BufferCore& mBC, const std::string& mode, const ros::Time & else ts.header.stamp = ros::Time(); - ts.child_frame_id = frame_prefix + frames[i]; + ts.child_frame_id = frame_prefix + frames[i]; if (i > 1) - ts.header.frame_id = frame_prefix + frames[i-1]; - else - ts.header.frame_id = frames[i-1]; + ts.header.frame_id = frame_prefix + frames[i-1]; + else + ts.header.frame_id = frames[i-1]; EXPECT_TRUE(mBC.setTransform(ts, "authority")); if (interpolation_space > ros::Duration()) From e5bc248093efe965b503131dc3a1e28601efb0f2 Mon Sep 17 00:00:00 2001 From: Patrick Beeson Date: Wed, 23 Oct 2019 20:32:59 -0500 Subject: [PATCH 6/9] Add output message and when data is dropped --- tf2/include/tf2/time_cache.h | 6 +++--- tf2/src/buffer_core.cpp | 5 +++-- tf2/src/cache.cpp | 16 ++++++++++++++-- tf2/src/static_cache.cpp | 2 +- 4 files changed, 21 insertions(+), 8 deletions(-) diff --git a/tf2/include/tf2/time_cache.h b/tf2/include/tf2/time_cache.h index 8ce925803..fe729c39d 100644 --- a/tf2/include/tf2/time_cache.h +++ b/tf2/include/tf2/time_cache.h @@ -60,7 +60,7 @@ class TimeCacheInterface virtual bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0)=0; //returns false if data unavailable (should be thrown as lookup exception /** \brief Insert data into the cache */ - virtual bool insertData(const TransformStorage& new_data)=0; + virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0)=0; /** @brief Clear the list of stored values */ virtual void clearList()=0; @@ -104,7 +104,7 @@ class TimeCache : public TimeCacheInterface /// Virtual methods virtual bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0); - virtual bool insertData(const TransformStorage& new_data); + virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0); virtual void clearList(); virtual CompactFrameID getParent(ros::Time time, std::string* error_str); virtual P_TimeAndFrameID getLatestTimeAndParent(); @@ -141,7 +141,7 @@ class StaticCache : public TimeCacheInterface /// Virtual methods virtual bool getData(ros::Time time, TransformStorage & data_out, std::string* error_str = 0); //returns false if data unavailable (should be thrown as lookup exception - virtual bool insertData(const TransformStorage& new_data); + virtual bool insertData(const TransformStorage& new_data, std::string* error_str = 0); virtual void clearList(); virtual CompactFrameID getParent(ros::Time time, std::string* error_str); virtual P_TimeAndFrameID getLatestTimeAndParent(); diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 75b827d1e..f01640611 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -268,13 +268,14 @@ bool BufferCore::setTransform(const geometry_msgs::TransformStamped& transform_i if (frame == NULL) frame = allocateFrame(frame_number, is_static); - if (frame->insertData(TransformStorage(stripped, lookupOrInsertFrameNumber(stripped.header.frame_id), frame_number))) + std::string error_string; + if (frame->insertData(TransformStorage(stripped, lookupOrInsertFrameNumber(stripped.header.frame_id), frame_number), &error_string)) { frame_authority_[frame_number] = authority; } else { - CONSOLE_BRIDGE_logWarn("TF_OLD_DATA ignoring data from the past for frame %s at time %g according to authority %s\nPossible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained", stripped.child_frame_id.c_str(), stripped.header.stamp.toSec(), authority.c_str()); + CONSOLE_BRIDGE_logWarn(error_string.c_str(), stripped.child_frame_id.c_str(), stripped.header.stamp.toSec(), authority.c_str()); return false; } } diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index 72252722c..2caf63605 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -242,7 +242,7 @@ CompactFrameID TimeCache::getParent(ros::Time time, std::string* error_str) return p_temp_1->frame_id_; } -bool TimeCache::insertData(const TransformStorage& new_data) +bool TimeCache::insertData(const TransformStorage& new_data, std::string* error_str) { L_TransformStorage::iterator storage_it = storage_.begin(); @@ -250,6 +250,12 @@ bool TimeCache::insertData(const TransformStorage& new_data) { if (storage_it->stamp_ > new_data.stamp_ + max_storage_time_) { + if (error_str) + { + std::stringstream ss; + ss << "TF_OLD_DATA ignoring data from the past for frame %s at time %lf according to authority %s\nPossible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained"; + *error_str = ss.str(); + } return false; } } @@ -263,7 +269,13 @@ bool TimeCache::insertData(const TransformStorage& new_data) } if (storage_it != storage_.end() && storage_it->stamp_ == new_data.stamp_) { - return true; + if (error_str) + { + std::stringstream ss; + ss << "TF_REPEATED_DATA ignoring data with redundant timestamp for frame %s at time %lf according to authority %s"; + *error_str = ss.str(); + } + return false; } else { diff --git a/tf2/src/static_cache.cpp b/tf2/src/static_cache.cpp index cb588c513..7297fb821 100644 --- a/tf2/src/static_cache.cpp +++ b/tf2/src/static_cache.cpp @@ -45,7 +45,7 @@ bool StaticCache::getData(ros::Time time, TransformStorage & data_out, std::stri return true; }; -bool StaticCache::insertData(const TransformStorage& new_data) +bool StaticCache::insertData(const TransformStorage& new_data, std::string* error_str) { storage_ = new_data; return true; From 3c2741364f85531b6f0c657d44b2afcb5ca1f33b Mon Sep 17 00:00:00 2001 From: Patrick Beeson Date: Wed, 23 Oct 2019 20:41:30 -0500 Subject: [PATCH 7/9] Slightly improve where message gets filled out with variables --- tf2/src/buffer_core.cpp | 2 +- tf2/src/cache.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index f01640611..067b8c1ed 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -275,7 +275,7 @@ bool BufferCore::setTransform(const geometry_msgs::TransformStamped& transform_i } else { - CONSOLE_BRIDGE_logWarn(error_string.c_str(), stripped.child_frame_id.c_str(), stripped.header.stamp.toSec(), authority.c_str()); + CONSOLE_BRIDGE_logWarn((error_string+" for frame %s at time %lf according to authority %s").c_str(), stripped.child_frame_id.c_str(), stripped.header.stamp.toSec(), authority.c_str()); return false; } } diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index 2caf63605..84fcfb1eb 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -253,7 +253,7 @@ bool TimeCache::insertData(const TransformStorage& new_data, std::string* error_ if (error_str) { std::stringstream ss; - ss << "TF_OLD_DATA ignoring data from the past for frame %s at time %lf according to authority %s\nPossible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained"; + ss << "TF_OLD_DATA ignoring data from the past (Possible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained)"; *error_str = ss.str(); } return false; @@ -272,7 +272,7 @@ bool TimeCache::insertData(const TransformStorage& new_data, std::string* error_ if (error_str) { std::stringstream ss; - ss << "TF_REPEATED_DATA ignoring data with redundant timestamp for frame %s at time %lf according to authority %s"; + ss << "TF_REPEATED_DATA ignoring data with redundant timestamp"; *error_str = ss.str(); } return false; From c5d70293ac1b2654fceb905d909e95049f7b2d98 Mon Sep 17 00:00:00 2001 From: Patrick Beeson Date: Wed, 23 Oct 2019 20:44:33 -0500 Subject: [PATCH 8/9] Linting --- tf2/src/cache.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index 84fcfb1eb..718951304 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -270,11 +270,11 @@ bool TimeCache::insertData(const TransformStorage& new_data, std::string* error_ if (storage_it != storage_.end() && storage_it->stamp_ == new_data.stamp_) { if (error_str) - { - std::stringstream ss; - ss << "TF_REPEATED_DATA ignoring data with redundant timestamp"; - *error_str = ss.str(); - } + { + std::stringstream ss; + ss << "TF_REPEATED_DATA ignoring data with redundant timestamp"; + *error_str = ss.str(); + } return false; } else From ea1432dc0e344204aa5c9d812ebbd839b5c1a33e Mon Sep 17 00:00:00 2001 From: Patrick Beeson Date: Wed, 23 Oct 2019 20:46:40 -0500 Subject: [PATCH 9/9] Tabs to spaces -- sorry --- tf2/src/cache.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tf2/src/cache.cpp b/tf2/src/cache.cpp index 718951304..b1a695a85 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -252,9 +252,9 @@ bool TimeCache::insertData(const TransformStorage& new_data, std::string* error_ { if (error_str) { - std::stringstream ss; - ss << "TF_OLD_DATA ignoring data from the past (Possible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained)"; - *error_str = ss.str(); + std::stringstream ss; + ss << "TF_OLD_DATA ignoring data from the past (Possible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained)"; + *error_str = ss.str(); } return false; }