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 ros#467:

ros#467

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

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

tf2/src/cache.cpp

@@ -277,8 +277,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;
@@ -294,6 +299,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();
 }