Detemplatize OrderedMultiQueue. (#75)

master
Damon Kohler 2016-10-18 15:09:29 +02:00 committed by GitHub
parent ccc26a7f4f
commit 653ce1706a
6 changed files with 173 additions and 169 deletions

View File

@ -123,15 +123,6 @@ google_library(common_mutex
common_time 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 google_library(common_port
USES_BOOST USES_BOOST
HDRS HDRS
@ -198,14 +189,6 @@ google_test(common_math_test
common_math 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 google_test(common_rate_timer_test
SRCS SRCS
rate_timer_test.cc rate_timer_test.cc

View File

@ -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

View File

@ -21,9 +21,9 @@ google_library(sensor_collator
collator.h collator.h
DEPENDS DEPENDS
common_make_unique common_make_unique
common_ordered_multi_queue
common_time common_time
sensor_data sensor_data
sensor_ordered_multi_queue
sensor_sensor_packet_period_histogram_builder sensor_sensor_packet_period_histogram_builder
) )
@ -78,6 +78,17 @@ google_library(sensor_laser
transform_transform 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 google_library(sensor_point_cloud
USES_EIGEN USES_EIGEN
USES_GLOG USES_GLOG
@ -140,6 +151,14 @@ google_test(sensor_laser_test
sensor_laser 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 google_test(sensor_point_cloud_test
SRCS SRCS
point_cloud_test.cc point_cloud_test.cc

View File

@ -26,30 +26,15 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/ordered_multi_queue.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/sensor/data.h" #include "cartographer/sensor/data.h"
#include "cartographer/sensor/ordered_multi_queue.h"
#include "cartographer/sensor/sensor_packet_period_histogram_builder.h" #include "cartographer/sensor/sensor_packet_period_histogram_builder.h"
#include "glog/logging.h" #include "glog/logging.h"
namespace cartographer { namespace cartographer {
namespace sensor { 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 { class Collator {
public: public:
using Callback = std::function<void(int64, std::unique_ptr<Data>)>; 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 std::unordered_set<string>& expected_sensor_ids,
const Callback callback) { const Callback callback) {
for (const auto& sensor_id : expected_sensor_ids) { 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); 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, 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_packet_period_histogram_builder_.Add(trajectory_id, timestamp,
sensor_id); 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. // Dispatches all queued sensor packets. May only be called once.
@ -111,18 +95,12 @@ class Collator {
} }
private: private:
struct Value {
int64 timestamp;
std::unique_ptr<Data> sensor_data;
};
// Queue keys are a pair of trajectory ID and sensor identifier. // 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. // 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 } // namespace sensor

View File

@ -14,8 +14,8 @@
* limitations under the License. * 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 <algorithm>
#include <map> #include <map>
@ -24,45 +24,58 @@
#include "cartographer/common/blocking_queue.h" #include "cartographer/common/blocking_queue.h"
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/sensor/data.h"
namespace cartographer { namespace cartographer {
namespace common { namespace sensor {
// Number of items that can be queued up before we LOG(WARNING). // Number of items that can be queued up before we LOG(WARNING).
const int kMaxQueueSize = 500; 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 { class OrderedMultiQueue {
public: 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 // Will wait to see at least one value for each 'expected_queue_keys' before
// dispatching the next smallest value across all queues. // dispatching the next smallest value across all queues.
explicit OrderedMultiQueue(const SortKeyType min_sort_key = SortKeyType()) OrderedMultiQueue() {}
: last_dispatched_key_(min_sort_key) {}
~OrderedMultiQueue() {} ~OrderedMultiQueue() {}
void AddQueue(const QueueKeyType& queue_key, Callback callback) { void AddQueue(const QueueKey& queue_key, Callback callback) {
CHECK(FindOrNull(queue_key) == nullptr); CHECK(FindOrNull(queue_key) == nullptr);
queues_[queue_key].callback = callback; queues_[queue_key].callback = callback;
} }
void MarkQueueAsFinished(const QueueKeyType& queue_key) { void MarkQueueAsFinished(const QueueKey& queue_key) {
auto& queue = FindOrDie(queue_key); auto& queue = FindOrDie(queue_key);
CHECK(!queue.finished); CHECK(!queue.finished);
queue.finished = true; queue.finished = true;
Dispatch(); Dispatch();
} }
bool HasQueue(const QueueKeyType& queue_key) { bool HasQueue(const QueueKey& queue_key) {
return queues_.count(queue_key) != 0; 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); auto* queue = FindOrNull(queue_key);
if (queue == nullptr) { if (queue == nullptr) {
// TODO(damonkohler): This will not work for every value of "queue_key". // 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 // Dispatches all remaining values in sorted order and removes the underlying
// queues. // queues.
void Flush() { void Flush() {
std::vector<QueueKeyType> unfinished_queues; std::vector<QueueKey> unfinished_queues;
for (auto& entry : queues_) { for (auto& entry : queues_) {
if (!entry.second.finished) { if (!entry.second.finished) {
unfinished_queues.push_back(entry.first); unfinished_queues.push_back(entry.first);
@ -90,16 +103,16 @@ class OrderedMultiQueue {
} }
// Returns the number of available values associated with 'queue_key'. // 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(); return FindOrDie(queue_key).queue.Size();
} }
private: private:
struct KeyValuePair { 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)) {} : 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 { struct Queue {
@ -109,14 +122,14 @@ class OrderedMultiQueue {
}; };
// Returns the queue with 'key' or LOG(FATAL). // Returns the queue with 'key' or LOG(FATAL).
Queue& FindOrDie(const QueueKeyType& key) { Queue& FindOrDie(const QueueKey& key) {
auto it = queues_.find(key); auto it = queues_.find(key);
CHECK(it != queues_.end()) << "Did not find '" << key << "'."; CHECK(it != queues_.end()) << "Did not find '" << key << "'.";
return it->second; return it->second;
} }
// Returns the queue with 'key' or nullptr. // Returns the queue with 'key' or nullptr.
Queue* FindOrNull(const QueueKeyType& key) { Queue* FindOrNull(const QueueKey& key) {
auto it = queues_.find(key); auto it = queues_.find(key);
if (it == queues_.end()) { if (it == queues_.end()) {
return nullptr; return nullptr;
@ -155,7 +168,8 @@ class OrderedMultiQueue {
return; return;
} }
last_dispatched_key_ = next_key_value_pair->sort_key; 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. // 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 } // namespace cartographer
#endif // CARTOGRAPHER_COMMON_ORDERED_MULTI_QUEUE_H_ #endif // CARTOGRAPHER_SENSOR_ORDERED_MULTI_QUEUE_H_

View File

@ -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