From 7dc3ab1e9e4e4f12ac6133c19f5aa586d8a6fc7e Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@google.com>
Date: Fri, 18 Nov 2016 12:49:33 +0100
Subject: [PATCH] Change dropping data before start to be idempotent. (#133)

We drop as much data from each queue as possible without changing the
start time that would be determined if we only saw the dispatched
data, i.e. we retain one piece of data not beyond the common start time.
This makes the process idempotent.
---
 cartographer/common/CMakeLists.txt            |  1 +
 .../common/configuration_files_test.cc        |  5 ++--
 cartographer/sensor/ordered_multi_queue.h     | 26 ++++++++++++++-----
 3 files changed, 24 insertions(+), 8 deletions(-)

diff --git a/cartographer/common/CMakeLists.txt b/cartographer/common/CMakeLists.txt
index a608b5f..aa1b978 100644
--- a/cartographer/common/CMakeLists.txt
+++ b/cartographer/common/CMakeLists.txt
@@ -175,6 +175,7 @@ google_test(common_configuration_files_test
   SRCS
     configuration_files_test.cc
   DEPENDS
+    common_config
     common_configuration_file_resolver
     common_lua_parameter_dictionary
     mapping_map_builder
diff --git a/cartographer/common/configuration_files_test.cc b/cartographer/common/configuration_files_test.cc
index 88b6d1a..dec4683 100644
--- a/cartographer/common/configuration_files_test.cc
+++ b/cartographer/common/configuration_files_test.cc
@@ -33,8 +33,9 @@ TEST(ConfigurationFilesTest, ValidateMapBuilderOptions) {
       return MAP_BUILDER)text";
   EXPECT_NO_FATAL_FAILURE({
     auto file_resolver = ::cartographer::common::make_unique<
-        ::cartographer::common::ConfigurationFileResolver>(std::vector<string>{
-        string(::cartographer::common::kSourceDirectory) + "/configuration_files"});
+        ::cartographer::common::ConfigurationFileResolver>(
+        std::vector<string>{string(::cartographer::common::kSourceDirectory) +
+                            "/configuration_files"});
     ::cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
         kCode, std::move(file_resolver));
     ::cartographer::mapping::CreateMapBuilderOptions(&lua_parameter_dictionary);
diff --git a/cartographer/sensor/ordered_multi_queue.h b/cartographer/sensor/ordered_multi_queue.h
index 0c35f8d..08b4bc3 100644
--- a/cartographer/sensor/ordered_multi_queue.h
+++ b/cartographer/sensor/ordered_multi_queue.h
@@ -168,13 +168,27 @@ class OrderedMultiQueue {
                   << common_start_time_ << "'.";
       }
 
-      if (next_data->time < common_start_time_) {
-        next_queue->queue.Pop();
-        continue;
+      if (next_data->time >= common_start_time_) {
+        // Happy case, we are beyond the 'common_start_time_' already.
+        last_dispatched_time_ = next_data->time;
+        next_queue->callback(next_queue->queue.Pop());
+      } else if (next_queue->queue.Size() < 2) {
+        if (!next_queue->finished) {
+          // We cannot decide whether to drop or dispatch this yet.
+          return;
+        }
+        last_dispatched_time_ = next_data->time;
+        next_queue->callback(next_queue->queue.Pop());
+      } else {
+        // We take a peek at the time after next data. If it also is not beyond
+        // 'common_start_time_' we drop 'next_data', otherwise we just found the
+        // first packet to dispatch from this queue.
+        std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop();
+        if (next_queue->queue.Peek<Data>()->time > common_start_time_) {
+          last_dispatched_time_ = next_data->time;
+          next_queue->callback(std::move(next_data_owner));
+        }
       }
-
-      last_dispatched_time_ = next_data->time;
-      next_queue->callback(next_queue->queue.Pop());
     }
   }