manually forward porting #68 to indigo

ros · Sep 18, 2014 · 619a132 · 619a132
1 parent 5a2b08b
commit 619a132
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 0 deletions.
diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h
@@ -71,6 +71,7 @@ class TransformListener
   ros::Subscriber message_subscriber_tf_static_;
   tf2::BufferCore& buffer_;
   bool using_dedicated_thread_;
+  ros::Time last_update_;
 
   void dedicatedListenerThread()
   {

diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp
@@ -101,6 +101,15 @@ void TransformListener::static_subscription_callback(const ros::MessageEvent<tf2
 
 void TransformListener::subscription_callback_impl(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt, bool is_static)
 {
+  ros::Time now = ros::Time::now();
+  if(now < last_update_){
+    ROS_WARN("Detected jump back in time. Clearing TF buffer.");
+    buffer_.clear();
+  }
+  last_update_ = now;
+
+
+
   const tf2_msgs::TFMessage& msg_in = *(msg_evt.getConstMessage());
   std::string authority = msg_evt.getPublisherName(); // lookup the authority
   for (unsigned int i = 0; i < msg_in.transforms.size(); i++)