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(); }