Detemplatize OrderedMultiQueue. (#75)

cartographer/common/CMakeLists.txt

@@ -123,15 +123,6 @@ google_library(common_mutex
     common_time
 )
 
-google_library(common_ordered_multi_queue
-  HDRS
-    ordered_multi_queue.h
-  DEPENDS
-    common_blocking_queue
-    common_make_unique
-    common_port
-)
-
 google_library(common_port
   USES_BOOST
   HDRS
@@ -198,14 +189,6 @@ google_test(common_math_test
     common_math
 )
 
-google_test(common_ordered_multi_queue_test
-  SRCS
-    ordered_multi_queue_test.cc
-  DEPENDS
-    common_make_unique
-    common_ordered_multi_queue
-)
-
 google_test(common_rate_timer_test
   SRCS
     rate_timer_test.cc

cartographer/common/ordered_multi_queue_test.cc

@@ -1,88 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "cartographer/common/ordered_multi_queue.h"
-
-#include <vector>
-
-#include "cartographer/common/make_unique.h"
-#include "gtest/gtest.h"
-
-namespace cartographer {
-namespace common {
-namespace {
-
-TEST(OrderedMultiQueue, Ordering) {
-  std::vector<int> values;
-  OrderedMultiQueue<int, int, int> queue;
-  for (int i : {1, 2, 3}) {
-    queue.AddQueue(i, [&values](std::unique_ptr<int> value) {
-      if (!values.empty()) {
-        EXPECT_GT(*value, values.back());
-      }
-      values.push_back(*value);
-    });
-  }
-  queue.Add(1, 4, common::make_unique<int>(4));
-  queue.Add(1, 5, common::make_unique<int>(5));
-  queue.Add(1, 6, common::make_unique<int>(6));
-  EXPECT_TRUE(values.empty());
-  queue.Add(2, 1, common::make_unique<int>(1));
-  EXPECT_TRUE(values.empty());
-  queue.Add(3, 2, common::make_unique<int>(2));
-  EXPECT_EQ(values.size(), 1);
-  queue.Add(2, 3, common::make_unique<int>(3));
-  EXPECT_EQ(values.size(), 2);
-  queue.Add(2, 7, common::make_unique<int>(7));
-  queue.Add(3, 8, common::make_unique<int>(8));
-  queue.Flush();
-
-  EXPECT_EQ(8, values.size());
-  for (size_t i = 0; i < values.size(); ++i) {
-    EXPECT_EQ(i + 1, values[i]);
-  }
-}
-
-TEST(OrderedMultiQueue, MarkQueueAsFinished) {
-  std::vector<int> values;
-  OrderedMultiQueue<int, int, int> queue;
-  for (int i : {1, 2, 3}) {
-    queue.AddQueue(i, [&values](std::unique_ptr<int> value) {
-      if (!values.empty()) {
-        EXPECT_GT(*value, values.back());
-      }
-      values.push_back(*value);
-    });
-  }
-  queue.Add(1, 1, common::make_unique<int>(1));
-  queue.Add(1, 2, common::make_unique<int>(2));
-  queue.Add(1, 3, common::make_unique<int>(3));
-  EXPECT_TRUE(values.empty());
-  queue.MarkQueueAsFinished(1);
-  EXPECT_TRUE(values.empty());
-  queue.MarkQueueAsFinished(2);
-  EXPECT_TRUE(values.empty());
-  queue.MarkQueueAsFinished(3);
-
-  EXPECT_EQ(3, values.size());
-  for (size_t i = 0; i < values.size(); ++i) {
-    EXPECT_EQ(i + 1, values[i]);
-  }
-}
-
-}  // namespace
-}  // namespace common
-}  // namespace cartographer

cartographer/sensor/CMakeLists.txt

@@ -21,9 +21,9 @@ google_library(sensor_collator
     collator.h
   DEPENDS
     common_make_unique
-    common_ordered_multi_queue
     common_time
     sensor_data
+    sensor_ordered_multi_queue
     sensor_sensor_packet_period_histogram_builder
 )
 
@@ -78,6 +78,17 @@ google_library(sensor_laser
     transform_transform
 )
 
+google_library(sensor_ordered_multi_queue
+  HDRS
+    ordered_multi_queue.h
+  DEPENDS
+    common_blocking_queue
+    common_make_unique
+    common_port
+    common_time
+    sensor_data
+)
+
 google_library(sensor_point_cloud
   USES_EIGEN
   USES_GLOG
@@ -140,6 +151,14 @@ google_test(sensor_laser_test
     sensor_laser
 )
 
+google_test(sensor_ordered_multi_queue_test
+  SRCS
+    ordered_multi_queue_test.cc
+  DEPENDS
+    common_make_unique
+    sensor_ordered_multi_queue
+)
+
 google_test(sensor_point_cloud_test
   SRCS
     point_cloud_test.cc

cartographer/sensor/collator.h

@@ -26,30 +26,15 @@
 #include "Eigen/Core"
 #include "Eigen/Geometry"
 #include "cartographer/common/make_unique.h"
-#include "cartographer/common/ordered_multi_queue.h"
 #include "cartographer/common/time.h"
 #include "cartographer/sensor/data.h"
+#include "cartographer/sensor/ordered_multi_queue.h"
 #include "cartographer/sensor/sensor_packet_period_histogram_builder.h"
 #include "glog/logging.h"
 
 namespace cartographer {
 namespace sensor {
 
-struct CollatorQueueKey {
-  int trajectory_id;
-  string sensor_id;
-
-  bool operator<(const CollatorQueueKey& other) const {
-    return std::forward_as_tuple(trajectory_id, sensor_id) <
-           std::forward_as_tuple(other.trajectory_id, other.sensor_id);
-  }
-};
-
-inline std::ostream& operator<<(std::ostream& out,
-                                const CollatorQueueKey& key) {
-  return out << '(' << key.trajectory_id << ", " << key.sensor_id << ')';
-}
-
 class Collator {
  public:
   using Callback = std::function<void(int64, std::unique_ptr<Data>)>;
@@ -65,9 +50,10 @@ class Collator {
                      const std::unordered_set<string>& expected_sensor_ids,
                      const Callback callback) {
     for (const auto& sensor_id : expected_sensor_ids) {
-      const auto queue_key = CollatorQueueKey{trajectory_id, sensor_id};
+      const auto queue_key = QueueKey{trajectory_id, sensor_id};
-      queue_.AddQueue(queue_key, [callback](std::unique_ptr<Value> value) {
+      queue_.AddQueue(queue_key, [callback](const common::Time time,
-        callback(value->timestamp, std::move(value->sensor_data));
+                                            std::unique_ptr<Data> data) {
+        callback(common::ToUniversal(time), std::move(data));
       });
       queue_keys_[trajectory_id].push_back(queue_key);
     }
@@ -80,17 +66,15 @@ class Collator {
     }
   }
 
-  // Adds 'sensor_data' for 'trajectory_id' to be collated. 'sensor_data' must
+  // Adds 'data' for 'trajectory_id' to be collated. 'data' must contain valid
-  // contain valid sensor data. Sensor packets with matching 'sensor_id' must be
+  // sensor data. Sensor packets with matching 'sensor_id' must be added in time
-  // added in time order.
+  // order.
   void AddSensorData(const int trajectory_id, const int64 timestamp,
-                     const string& sensor_id,
+                     const string& sensor_id, std::unique_ptr<Data> data) {
-                     std::unique_ptr<Data> sensor_data) {
     sensor_packet_period_histogram_builder_.Add(trajectory_id, timestamp,
                                                 sensor_id);
-    queue_.Add(
+    queue_.Add(QueueKey{trajectory_id, sensor_id},
-        CollatorQueueKey{trajectory_id, sensor_id}, timestamp,
+               common::FromUniversal(timestamp), std::move(data));
-        common::make_unique<Value>(Value{timestamp, std::move(sensor_data)}));
   }
 
   // Dispatches all queued sensor packets. May only be called once.
@@ -111,18 +95,12 @@ class Collator {
   }
 
  private:
-  struct Value {
-    int64 timestamp;
-    std::unique_ptr<Data> sensor_data;
-  };
-
   // Queue keys are a pair of trajectory ID and sensor identifier.
-  common::OrderedMultiQueue<CollatorQueueKey, int64, Value> queue_;
+  OrderedMultiQueue queue_;
 
   // Map of trajectory ID to all associated QueueKeys.
-  std::unordered_map<int, std::vector<CollatorQueueKey>> queue_keys_;
+  std::unordered_map<int, std::vector<QueueKey>> queue_keys_;
-  sensor::SensorPacketPeriodHistogramBuilder
+  SensorPacketPeriodHistogramBuilder sensor_packet_period_histogram_builder_;
-      sensor_packet_period_histogram_builder_;
 };
 
 }  // namespace sensor

cartographer/sensor/ordered_multi_queue.h

@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
-#ifndef CARTOGRAPHER_COMMON_ORDERED_MULTI_QUEUE_H_
+#ifndef CARTOGRAPHER_SENSOR_ORDERED_MULTI_QUEUE_H_
-#define CARTOGRAPHER_COMMON_ORDERED_MULTI_QUEUE_H_
+#define CARTOGRAPHER_SENSOR_ORDERED_MULTI_QUEUE_H_
 
 #include <algorithm>
 #include <map>
@@ -24,45 +24,58 @@
 #include "cartographer/common/blocking_queue.h"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/port.h"
+#include "cartographer/common/time.h"
+#include "cartographer/sensor/data.h"
 
 namespace cartographer {
-namespace common {
+namespace sensor {
 
 // Number of items that can be queued up before we LOG(WARNING).
 const int kMaxQueueSize = 500;
 
-// Maintains multiple queues of sorted values and dispatches merge sorted
+struct QueueKey {
-// values. This class is thread-compatible.
+  int trajectory_id;
-template <typename QueueKeyType, typename SortKeyType, typename ValueType>
+  string sensor_id;
+
+  bool operator<(const QueueKey& other) const {
+    return std::forward_as_tuple(trajectory_id, sensor_id) <
+           std::forward_as_tuple(other.trajectory_id, other.sensor_id);
+  }
+};
+
+inline std::ostream& operator<<(std::ostream& out, const QueueKey& key) {
+  return out << '(' << key.trajectory_id << ", " << key.sensor_id << ')';
+}
+
+// Maintains multiple queues of sorted sensor data and dispatches it in merge
+// sorted order. This class is thread-compatible.
 class OrderedMultiQueue {
  public:
-  using Callback = std::function<void(std::unique_ptr<ValueType>)>;
+  using Callback = std::function<void(common::Time, std::unique_ptr<Data>)>;
 
   // Will wait to see at least one value for each 'expected_queue_keys' before
   // dispatching the next smallest value across all queues.
-  explicit OrderedMultiQueue(const SortKeyType min_sort_key = SortKeyType())
+  OrderedMultiQueue() {}
-      : last_dispatched_key_(min_sort_key) {}
-
   ~OrderedMultiQueue() {}
 
-  void AddQueue(const QueueKeyType& queue_key, Callback callback) {
+  void AddQueue(const QueueKey& queue_key, Callback callback) {
     CHECK(FindOrNull(queue_key) == nullptr);
     queues_[queue_key].callback = callback;
   }
 
-  void MarkQueueAsFinished(const QueueKeyType& queue_key) {
+  void MarkQueueAsFinished(const QueueKey& queue_key) {
     auto& queue = FindOrDie(queue_key);
     CHECK(!queue.finished);
     queue.finished = true;
     Dispatch();
   }
 
-  bool HasQueue(const QueueKeyType& queue_key) {
+  bool HasQueue(const QueueKey& queue_key) {
     return queues_.count(queue_key) != 0;
   }
 
-  void Add(const QueueKeyType& queue_key, const SortKeyType& sort_key,
+  void Add(const QueueKey& queue_key, const common::Time& sort_key,
-           std::unique_ptr<ValueType> value) {
+           std::unique_ptr<Data> value) {
     auto* queue = FindOrNull(queue_key);
     if (queue == nullptr) {
       // TODO(damonkohler): This will not work for every value of "queue_key".
@@ -78,7 +91,7 @@ class OrderedMultiQueue {
   // Dispatches all remaining values in sorted order and removes the underlying
   // queues.
   void Flush() {
-    std::vector<QueueKeyType> unfinished_queues;
+    std::vector<QueueKey> unfinished_queues;
     for (auto& entry : queues_) {
       if (!entry.second.finished) {
         unfinished_queues.push_back(entry.first);
@@ -90,16 +103,16 @@ class OrderedMultiQueue {
   }
 
   // Returns the number of available values associated with 'queue_key'.
-  int num_available(const QueueKeyType& queue_key) {
+  int num_available(const QueueKey& queue_key) {
     return FindOrDie(queue_key).queue.Size();
   }
 
  private:
   struct KeyValuePair {
-    KeyValuePair(const SortKeyType& sort_key, std::unique_ptr<ValueType> value)
+    KeyValuePair(const common::Time& sort_key, std::unique_ptr<Data> value)
         : sort_key(sort_key), value(std::move(value)) {}
-    SortKeyType sort_key;
+    common::Time sort_key;
-    std::unique_ptr<ValueType> value;
+    std::unique_ptr<Data> value;
   };
 
   struct Queue {
@@ -109,14 +122,14 @@ class OrderedMultiQueue {
   };
 
   // Returns the queue with 'key' or LOG(FATAL).
-  Queue& FindOrDie(const QueueKeyType& key) {
+  Queue& FindOrDie(const QueueKey& key) {
     auto it = queues_.find(key);
     CHECK(it != queues_.end()) << "Did not find '" << key << "'.";
     return it->second;
   }
 
   // Returns the queue with 'key' or nullptr.
-  Queue* FindOrNull(const QueueKeyType& key) {
+  Queue* FindOrNull(const QueueKey& key) {
     auto it = queues_.find(key);
     if (it == queues_.end()) {
       return nullptr;
@@ -155,7 +168,8 @@ class OrderedMultiQueue {
         return;
       }
       last_dispatched_key_ = next_key_value_pair->sort_key;
-      next_queue->callback(std::move(next_queue->queue.Pop()->value));
+      next_queue->callback(last_dispatched_key_,
+                           std::move(next_queue->queue.Pop()->value));
     }
   }
 
@@ -168,12 +182,12 @@ class OrderedMultiQueue {
   }
 
   // Used to verify that values are dispatched in sorted order.
-  SortKeyType last_dispatched_key_;
+  common::Time last_dispatched_key_ = common::Time::min();
 
-  std::map<QueueKeyType, Queue> queues_;
+  std::map<QueueKey, Queue> queues_;
 };
 
-}  // namespace common
+}  // namespace sensor
 }  // namespace cartographer
 
-#endif  // CARTOGRAPHER_COMMON_ORDERED_MULTI_QUEUE_H_
+#endif  // CARTOGRAPHER_SENSOR_ORDERED_MULTI_QUEUE_H_

cartographer/sensor/ordered_multi_queue_test.cc

@@ -0,0 +1,98 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/sensor/ordered_multi_queue.h"
+
+#include <vector>
+
+#include "cartographer/common/make_unique.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace sensor {
+namespace {
+
+class OrderedMultiQueueTest : public ::testing::Test {
+ protected:
+  const QueueKey kFirst{1, "foo"};
+  const QueueKey kSecond{1, "bar"};
+  const QueueKey kThird{2, "bar"};
+
+  void SetUp() {
+    for (const auto& queue_key : {kFirst, kSecond, kThird}) {
+      queue_.AddQueue(queue_key, [this](const common::Time time,
+                                        std::unique_ptr<Data> data) {
+        EXPECT_EQ(common::ToUniversal(time), data->imu.linear_acceleration.x());
+        if (!values_.empty()) {
+          EXPECT_GT(data->imu.linear_acceleration.x(),
+                    values_.back().imu.linear_acceleration.x());
+        }
+        values_.push_back(*data);
+      });
+    }
+  }
+
+  std::unique_ptr<Data> MakeImu(const int ordinal) {
+    return common::make_unique<Data>(
+        "unused_frame_id",
+        Data::Imu{ordinal * Eigen::Vector3d::UnitX(), Eigen::Vector3d::Zero()});
+  }
+
+  std::vector<Data> values_;
+  OrderedMultiQueue queue_;
+};
+
+TEST_F(OrderedMultiQueueTest, Ordering) {
+  queue_.Add(kFirst, common::FromUniversal(4), MakeImu(4));
+  queue_.Add(kFirst, common::FromUniversal(5), MakeImu(5));
+  queue_.Add(kFirst, common::FromUniversal(6), MakeImu(6));
+  EXPECT_TRUE(values_.empty());
+  queue_.Add(kSecond, common::FromUniversal(1), MakeImu(1));
+  EXPECT_TRUE(values_.empty());
+  queue_.Add(kThird, common::FromUniversal(2), MakeImu(2));
+  EXPECT_EQ(values_.size(), 1);
+  queue_.Add(kSecond, common::FromUniversal(3), MakeImu(3));
+  EXPECT_EQ(values_.size(), 2);
+  queue_.Add(kSecond, common::FromUniversal(7), MakeImu(7));
+  queue_.Add(kThird, common::FromUniversal(8), MakeImu(8));
+  queue_.Flush();
+
+  EXPECT_EQ(8, values_.size());
+  for (size_t i = 0; i < values_.size(); ++i) {
+    EXPECT_EQ(i + 1, values_[i].imu.linear_acceleration.x());
+  }
+}
+
+TEST_F(OrderedMultiQueueTest, MarkQueueAsFinished) {
+  queue_.Add(kFirst, common::FromUniversal(1), MakeImu(1));
+  queue_.Add(kFirst, common::FromUniversal(2), MakeImu(2));
+  queue_.Add(kFirst, common::FromUniversal(3), MakeImu(3));
+  EXPECT_TRUE(values_.empty());
+  queue_.MarkQueueAsFinished(kFirst);
+  EXPECT_TRUE(values_.empty());
+  queue_.MarkQueueAsFinished(kSecond);
+  EXPECT_TRUE(values_.empty());
+  queue_.MarkQueueAsFinished(kThird);
+
+  EXPECT_EQ(3, values_.size());
+  for (size_t i = 0; i < values_.size(); ++i) {
+    EXPECT_EQ(i + 1, values_[i].imu.linear_acceleration.x());
+  }
+}
+
+}  // namespace
+}  // namespace sensor
+}  // namespace cartographer