From 5dabdd5d8a47eee152b046806f4f60ce7c7789b9 Mon Sep 17 00:00:00 2001 From: "C. Andy Martin" Date: Wed, 18 Aug 2021 19:58:14 -0400 Subject: [PATCH] throttle TF_REPEATED_DATA to 10 seconds The TF_REPEATED_DATA error, while important, can be overly verbose, sometimes flooding the terminal scrollback buffer such that no other output can be found. Throttle the error to 10 seconds. Implement the throttling directly since console bridge does not implement throttling. Because ros::Time may not yet be initialized when calling insertData, use the stamp from the message as "now". For more information including the justification for the warning and the justification for a 10 second throttle, read the discussion on issue #467: https://github.com/ros/geometry2/issues/467 --- tf2/include/tf2/time_cache.h | 2 +- tf2/src/buffer_core.cpp | 5 ++++- tf2/src/cache.cpp | 9 ++++++++- 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/tf2/include/tf2/time_cache.h b/tf2/include/tf2/time_cache.h index 14311129b..639711d3d 100644 --- a/tf2/include/tf2/time_cache.h +++ b/tf2/include/tf2/time_cache.h @@ -118,7 +118,7 @@ class TimeCache : public TimeCacheInterface L_TransformStorage storage_; ros::Duration max_storage_time_; - + ros::Time last_repeated_warn_time_; /// A helper function for getData //Assumes storage is already locked for it diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 6eed12d09..5b3ef62d0 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -275,7 +275,10 @@ bool BufferCore::setTransform(const geometry_msgs::TransformStamped& transform_i } else { - 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()); + if (error_string.size()) + { + 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 f9614c792..ac5d59a98 100644 --- a/tf2/src/cache.cpp +++ b/tf2/src/cache.cpp @@ -275,8 +275,13 @@ bool TimeCache::insertData(const TransformStorage& new_data, std::string* error_ } if (storage_it != storage_.end() && storage_it->stamp_ == new_data.stamp_) { - if (error_str) + // Throttle TF_REPEATED_DATA error message to 10 seconds. + // Console bridge does not have throttling, so implement throttling here. + // Because insertData may be called prior to ros::Time being initialized, + // use the stamp from the message as "now". + if (error_str && new_data.stamp_ > last_repeated_warn_time_ + ros::Duration(10.0)) { + last_repeated_warn_time_ = new_data.stamp_; *error_str = "TF_REPEATED_DATA ignoring data with redundant timestamp"; } return false; @@ -292,6 +297,8 @@ bool TimeCache::insertData(const TransformStorage& new_data, std::string* error_ void TimeCache::clearList() { + // Reset the TF_REPEATED_DATA error message throttle. + last_repeated_warn_time_ = ros::Time(0.0); storage_.clear(); }