12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
cartographer/common/CMakeLists.txt
View File

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

88
cartographer/common/ordered_multi_queue_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

21
cartographer/sensor/CMakeLists.txt
View File

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

50
cartographer/sensor/collator.h
View File

@ -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};
queue_.AddQueue(queue_key, [callback](std::unique_ptr<Value> value) {
callback(value->timestamp, std::move(value->sensor_data));
const auto queue_key = QueueKey{trajectory_id, sensor_id};
queue_.AddQueue(queue_key, [callback](const common::Time time,
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
// contain valid sensor data. Sensor packets with matching 'sensor_id' must be
// added in time order.
// Adds 'data' for 'trajectory_id' to be collated. 'data' must contain valid
// sensor data. Sensor packets with matching 'sensor_id' must be added in time
// order.
void AddSensorData(const int trajectory_id, const int64 timestamp,
const string& sensor_id,
std::unique_ptr<Data> sensor_data) {
const string& sensor_id, std::unique_ptr<Data> data) {
sensor_packet_period_histogram_builder_.Add(trajectory_id, timestamp,
sensor_id);
queue_.Add(
CollatorQueueKey{trajectory_id, sensor_id}, timestamp,
common::make_unique<Value>(Value{timestamp, std::move(sensor_data)}));
queue_.Add(QueueKey{trajectory_id, sensor_id},
common::FromUniversal(timestamp), std::move(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_;
sensor::SensorPacketPeriodHistogramBuilder
sensor_packet_period_histogram_builder_;
std::unordered_map<int, std::vector<QueueKey>> queue_keys_;
SensorPacketPeriodHistogramBuilder sensor_packet_period_histogram_builder_;
};
} // namespace sensor

68
cartographer/common/ordered_multi_queue.h → cartographer/sensor/ordered_multi_queue.h
View File

@ -14,8 +14,8 @@
* limitations under the License.
*/
#ifndef CARTOGRAPHER_COMMON_ORDERED_MULTI_QUEUE_H_
#define CARTOGRAPHER_COMMON_ORDERED_MULTI_QUEUE_H_
#ifndef CARTOGRAPHER_SENSOR_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
// values. This class is thread-compatible.
template <typename QueueKeyType, typename SortKeyType, typename ValueType>
struct QueueKey {
int trajectory_id;
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())
: 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);
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,
std::unique_ptr<ValueType> value) {
void Add(const QueueKey& queue_key, const common::Time& sort_key,
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;
std::unique_ptr<ValueType> value;
common::Time sort_key;
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_

98
cartographer/sensor/ordered_multi_queue_test.cc Normal file
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