Don't insert a TF frame if one of the same timestamp already exists, instead just overwrite it.

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];
+          ts.header.frame_id = frame_prefix + frames[i-1];
         else
-          ts.child_frame_id = frames[i]; // connect first frame
+          ts.header.frame_id = frames[i-1];
+
         EXPECT_TRUE(mBC.setTransform(ts, "authority"));
         if (interpolation_space > ros::Duration())
         {

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

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+" 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

@@ -242,14 +242,20 @@ 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();
 
   if(storage_it != storage_.end())
   {
     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 (Possible reasons are listed at http://wiki.ros.org/tf/Errors%%20explained)";
+        *error_str = ss.str();
+      }
       return false;
     }
   }
@@ -261,7 +267,20 @@ 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_)
+  {
+    if (error_str)
+    {
+      std::stringstream ss;
+      ss << "TF_REPEATED_DATA ignoring data with redundant timestamp";
+      *error_str = ss.str();
+    }
+    return false;
+  }
+  else
+  {
+    storage_.insert(storage_it, new_data);
+  }
 
   pruneList();
   return true;

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;