From 0e15fb9a8755fc64e0aef0a6b7af2f527c49c946 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 27 Sep 2016 16:39:25 +0200 Subject: [PATCH] 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 | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index 16d79f6..be95611 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/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,