Quickly skip over data at the beginning. (#70)

Sensor data that precedes the published tf transforms can likely never be
transformed, so it should be skipped without delay.

cartographer_ros/src/cartographer_node_main.cc

@@ -283,9 +283,20 @@ Node::~Node() {
 
 Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time,
                                                const string& frame_id) {
-  return ToRigid3d(tf_buffer_.lookupTransform(
-      options_.tracking_frame, frame_id, ToRos(time),
-      ::ros::Duration(options_.lookup_transform_timeout_sec)));
+  ::ros::Duration timeout(options_.lookup_transform_timeout_sec);
+  const ::ros::Time latest_tf_time =
+      tf_buffer_
+          .lookupTransform(options_.tracking_frame, frame_id, ::ros::Time(0.),
+                           timeout)
+          .header.stamp;
+  const ::ros::Time requested_time = ToRos(time);
+  if (latest_tf_time >= requested_time) {
+    // We already have newer data, so we do not wait. Otherwise, we would wait
+    // for the full 'timeout' even if we ask for data that is too old.
+    timeout = ::ros::Duration(0.);
+  }
+  return ToRigid3d(tf_buffer_.lookupTransform(options_.tracking_frame, frame_id,
+                                              requested_time, timeout));
 }
 
 void Node::AddOdometry(const int64 timestamp, const string& frame_id,